100 lines
5.3 KiB
C
100 lines
5.3 KiB
C
#include "sim_io.h"
|
|
|
|
#include "sim_hal.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static sim_posemath_pose pose(double x, double y, double z) {
|
|
sim_posemath_pose value = {{x, y, z}, 0, 0, 0, 0, 0, 0};
|
|
return value;
|
|
}
|
|
|
|
int main(void) {
|
|
sim_io io = {0};
|
|
sim_io_state state = {0};
|
|
sim_hal_value pin = {0};
|
|
char canonical[2048];
|
|
uint64_t hash;
|
|
uint64_t repeat_hash;
|
|
int invalid_pin_status;
|
|
int limit_status;
|
|
int spindle_step_status;
|
|
int tool_busy_status;
|
|
int probe_status;
|
|
int estop_command_status;
|
|
int estop_recover_status;
|
|
int recover_status;
|
|
int fault_clear_status;
|
|
|
|
sim_hal_reset();
|
|
assert(sim_io_init(&io, 2, 32, 0.1, 600.0, 5.0) == SIM_IO_OK);
|
|
assert(sim_hal_pin_count() >= 58 && sim_hal_function_count() == 1);
|
|
assert(sim_hal_get_pin("motion.probe-input", &pin) == SIM_HAL_OK && pin.type == SIM_HAL_BIT);
|
|
invalid_pin_status = sim_hal_set_pin("spindle.0.speed-out", (sim_hal_value){SIM_HAL_BIT, {.bit = 1}});
|
|
assert(invalid_pin_status == SIM_HAL_ERR_TYPE);
|
|
limit_status = sim_io_spindle_command(&io, 0, 100001.0, 1);
|
|
assert(limit_status == SIM_IO_ERR_LIMIT);
|
|
assert(sim_io_spindle_command(&io, 0, 1200.0, 1) == SIM_IO_OK);
|
|
assert(sim_io_spindle_adjust(&io, 0, -100.0) == SIM_IO_OK && io.state.spindles[0].command_rpm == 1100.0);
|
|
assert(sim_io_spindle_adjust(&io, 0, 100.0) == SIM_IO_OK && io.state.spindles[0].command_rpm == 1200.0);
|
|
spindle_step_status = sim_io_step(&io, 10);
|
|
assert(spindle_step_status == SIM_IO_OK && io.state.spindles[0].speed_rpm == 600.0);
|
|
assert(sim_io_step(&io, 10) == SIM_IO_OK && io.state.spindles[0].at_speed == 1);
|
|
|
|
assert(sim_io_tool_prepare(&io, 7, 3) == SIM_IO_OK);
|
|
assert(sim_io_tool_set_prepared(&io, 1) == SIM_IO_OK);
|
|
tool_busy_status = sim_io_tool_change(&io);
|
|
assert(tool_busy_status == SIM_IO_ERR_BUSY);
|
|
assert(sim_io_spindle_command(&io, 0, 0.0, 0) == SIM_IO_OK);
|
|
assert(sim_io_step(&io, 20) == SIM_IO_OK);
|
|
assert(sim_io_tool_change(&io) == SIM_IO_OK);
|
|
assert(sim_io_tool_set_changed(&io, 1) == SIM_IO_OK && io.state.current_tool == 7);
|
|
|
|
assert(sim_io_probe_arm(&io, 0) == SIM_IO_OK);
|
|
assert(sim_io_probe_disarm(&io) == SIM_IO_OK && io.state.probe.state == SIM_IO_PROBE_IDLE);
|
|
assert(sim_io_probe_arm(&io, 0) == SIM_IO_OK);
|
|
probe_status = sim_io_probe_update(&io, &(sim_posemath_pose){{1.0, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 1);
|
|
assert(probe_status == SIM_IO_OK && io.state.probe.state == SIM_IO_PROBE_TRIPPED);
|
|
assert(sim_io_probe_update(&io, &(sim_posemath_pose){{NAN, 2.0, 3.0}, 0, 0, 0, 0, 0, 0}, 0) == SIM_IO_ERR_NONFINITE);
|
|
|
|
assert(sim_io_set_estop(&io, 1) == SIM_IO_OK);
|
|
estop_command_status = sim_io_spindle_command(&io, 0, 100.0, 1);
|
|
estop_recover_status = sim_io_recover(&io);
|
|
assert(estop_command_status == SIM_IO_ERR_ESTOP && estop_recover_status == SIM_IO_ERR_ESTOP);
|
|
assert(sim_io_set_estop(&io, 0) == SIM_IO_OK);
|
|
recover_status = sim_io_recover(&io);
|
|
assert(recover_status == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 1);
|
|
assert(sim_io_set_power(&io, 0) == SIM_IO_OK && io.state.ready == 0);
|
|
assert(sim_io_spindle_command(&io, 0, 100.0, 1) == SIM_IO_ERR_NOT_READY);
|
|
assert(sim_io_set_power(&io, 1) == SIM_IO_OK && io.state.ready == 1 && io.state.recovery_epoch == 2);
|
|
assert(sim_io_spindle_brake(&io, 0, 0) == SIM_IO_OK);
|
|
assert(sim_io_spindle_command(&io, 0, 600.0, 1) == SIM_IO_OK);
|
|
assert(sim_io_spindle_brake(&io, 0, 1) == SIM_IO_OK && io.state.spindles[0].brake == 1 && io.state.spindles[0].command_rpm == 0.0);
|
|
|
|
assert(sim_io_tool_set_fault(&io, 1, 42) == SIM_IO_OK);
|
|
fault_clear_status = sim_io_tool_clear_fault(&io);
|
|
assert(fault_clear_status == SIM_IO_ERR_FAULT);
|
|
assert(sim_io_tool_set_fault(&io, 0, 0) == SIM_IO_OK);
|
|
assert(sim_io_tool_clear_fault(&io) == SIM_IO_OK);
|
|
hash = sim_io_state_hash(&io);
|
|
repeat_hash = sim_io_state_hash(&io);
|
|
assert(hash == repeat_hash);
|
|
assert(sim_io_canonical(&io, canonical, sizeof(canonical)) > 1 && canonical[0] == '{');
|
|
assert(sim_io_canonical(&io, 0, 0) == strlen(canonical) + 1);
|
|
assert(sim_hal_get_pin("iocontrol.0.tool-number", &pin) == SIM_HAL_OK && pin.value.s32 == 7);
|
|
assert(sim_hal_get_pin("spindle.0.brake", &pin) == SIM_HAL_OK && pin.value.bit == 1);
|
|
printf("{\"schema\":\"io-samples.v1\",\"pin\":{\"invalid_type\":%d,\"count\":%u,\"functions\":%u},\"spindle\":{\"limit\":%d,\"step\":%d,\"speed\":%.3f,\"at_speed\":%u,\"tool_busy\":%d},\"tool\":{\"state\":%u,\"current\":%d,\"fault_clear\":%d},\"probe\":{\"status\":%d,\"state\":%u,\"position\":[%.3f,%.3f,%.3f]},\"estop\":{\"command\":%d,\"recover_before_clear\":%d,\"recover\":%d},\"state_hash\":%llu,\"repeat_hash\":%llu,\"canonical\":%s}\n",
|
|
invalid_pin_status, sim_hal_pin_count(), sim_hal_function_count(), limit_status,
|
|
spindle_step_status, io.state.spindles[0].speed_rpm, io.state.spindles[0].at_speed,
|
|
tool_busy_status, io.state.tool_state, io.state.current_tool, fault_clear_status,
|
|
probe_status, io.state.probe.state, io.state.probe.position.tran.x,
|
|
io.state.probe.position.tran.y, io.state.probe.position.tran.z,
|
|
estop_command_status, estop_recover_status, recover_status,
|
|
(unsigned long long)hash, (unsigned long long)repeat_hash, canonical);
|
|
(void)pose;
|
|
return 0;
|
|
}
|