591 lines
28 KiB
C
591 lines
28 KiB
C
#include "sim_io.h"
|
|
|
|
#include "sim_hal.h"
|
|
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
static uint64_t io_fnv1a64(const unsigned char *bytes, size_t length) {
|
|
uint64_t hash = UINT64_C(1469598103934665603);
|
|
size_t index;
|
|
for (index = 0; index < length; ++index) {
|
|
hash ^= bytes[index];
|
|
hash *= UINT64_C(1099511628211);
|
|
}
|
|
return hash;
|
|
}
|
|
|
|
static double normalized(double value) {
|
|
return isfinite(value) ? nearbyint(value * 1000000000.0) / 1000000000.0 : value;
|
|
}
|
|
|
|
static int finite_pose(const sim_posemath_pose *pose) {
|
|
const double *values;
|
|
uint32_t index;
|
|
if (pose == 0) return 0;
|
|
values = &pose->tran.x;
|
|
for (index = 0; index < 9; ++index) if (!isfinite(values[index])) return 0;
|
|
return 1;
|
|
}
|
|
|
|
static int valid_io(const sim_io *io) {
|
|
return io != 0 && io->state.spindle_count > 0 &&
|
|
io->state.spindle_count <= SIM_IO_MAX_SPINDLES;
|
|
}
|
|
|
|
uint32_t sim_io_size(void) { return (uint32_t)sizeof(sim_io); }
|
|
|
|
static int valid_spindle(const sim_io *io, uint32_t spindle) {
|
|
return valid_io(io) && spindle < io->state.spindle_count &&
|
|
io->state.spindles[spindle].registered;
|
|
}
|
|
|
|
static void set_last(sim_io *io, int result) {
|
|
if (io != 0) io->state.last_error = result;
|
|
}
|
|
|
|
static int register_pin(const char *name, uint32_t type) {
|
|
int result = sim_hal_register_pin(name, type);
|
|
if (result == SIM_HAL_OK) return SIM_IO_OK;
|
|
if (result == SIM_HAL_ERR_DUPLICATE) {
|
|
sim_hal_value existing = {0};
|
|
if (sim_hal_get_pin(name, &existing) == SIM_HAL_OK && existing.type == type) return SIM_IO_OK;
|
|
}
|
|
return SIM_IO_ERR_PIN;
|
|
}
|
|
|
|
static int register_io_pins(sim_io *io) {
|
|
static const struct { const char *name; uint32_t type; } fixed[] = {
|
|
{"motion.probe-input", SIM_HAL_BIT},
|
|
{"iocontrol.0.user-enable-out", SIM_HAL_BIT},
|
|
{"iocontrol.0.emc-enable-in", SIM_HAL_BIT},
|
|
{"iocontrol.0.user-request-enable", SIM_HAL_BIT},
|
|
{"iocontrol.0.coolant-mist", SIM_HAL_BIT},
|
|
{"iocontrol.0.coolant-flood", SIM_HAL_BIT},
|
|
{"iocontrol.0.tool-prepare", SIM_HAL_BIT},
|
|
{"iocontrol.0.tool-prep-number", SIM_HAL_S32},
|
|
{"iocontrol.0.tool-prep-pocket", SIM_HAL_S32},
|
|
{"iocontrol.0.tool-prep-index", SIM_HAL_S32},
|
|
{"iocontrol.0.tool-number", SIM_HAL_S32},
|
|
{"iocontrol.0.tool-prepared", SIM_HAL_BIT},
|
|
{"iocontrol.0.tool-change", SIM_HAL_BIT},
|
|
{"iocontrol.0.tool-changed", SIM_HAL_BIT},
|
|
{"iocontrol.0.toolchanger-fault", SIM_HAL_BIT},
|
|
{"iocontrol.0.toolchanger-fault-ack", SIM_HAL_BIT},
|
|
{"iocontrol.0.toolchanger-reason", SIM_HAL_S32},
|
|
{"iocontrol.0.toolchanger-faulted", SIM_HAL_BIT},
|
|
{"iocontrol.0.toolchanger-clear-fault", SIM_HAL_BIT},
|
|
{"iocontrol.0.emc-abort", SIM_HAL_BIT},
|
|
{"iocontrol.0.emc-abort-ack", SIM_HAL_BIT},
|
|
{"iocontrol.0.emc-reason", SIM_HAL_S32},
|
|
{"iocontrol.0.start-change", SIM_HAL_BIT},
|
|
{"iocontrol.0.start-change-ack", SIM_HAL_BIT},
|
|
{"iocontrol.0.state", SIM_HAL_S32},
|
|
};
|
|
uint32_t index;
|
|
char name[64];
|
|
int result;
|
|
result = sim_hal_register_thread("sim-io-servo", 1);
|
|
if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
|
|
result = sim_hal_register_function("sim-io-servo", "sim-io");
|
|
if (result != SIM_HAL_OK && result != SIM_HAL_ERR_DUPLICATE) return SIM_IO_ERR_PIN;
|
|
for (index = 0; index < sizeof(fixed) / sizeof(fixed[0]); ++index) {
|
|
result = register_pin(fixed[index].name, fixed[index].type);
|
|
if (result != SIM_IO_OK) return result;
|
|
}
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
static const struct { const char *suffix; uint32_t type; } spindle_pins[] = {
|
|
{"on", SIM_HAL_BIT}, {"forward", SIM_HAL_BIT}, {"reverse", SIM_HAL_BIT},
|
|
{"brake", SIM_HAL_BIT}, {"speed-out", SIM_HAL_FLOAT},
|
|
{"speed-out-abs", SIM_HAL_FLOAT}, {"speed-out-rps", SIM_HAL_FLOAT},
|
|
{"speed-out-rps-abs", SIM_HAL_FLOAT}, {"speed-cmd-rps", SIM_HAL_FLOAT},
|
|
{"inhibit", SIM_HAL_BIT}, {"amp-fault-in", SIM_HAL_BIT},
|
|
{"orient-angle", SIM_HAL_FLOAT}, {"orient-mode", SIM_HAL_S32},
|
|
{"orient", SIM_HAL_BIT}, {"locked", SIM_HAL_BIT},
|
|
{"is-oriented", SIM_HAL_BIT}, {"orient-fault", SIM_HAL_S32},
|
|
{"revs", SIM_HAL_FLOAT}, {"speed-in", SIM_HAL_FLOAT},
|
|
{"at-speed", SIM_HAL_BIT}, {"index-enable", SIM_HAL_BIT},
|
|
};
|
|
uint32_t pin;
|
|
for (pin = 0; pin < sizeof(spindle_pins) / sizeof(spindle_pins[0]); ++pin) {
|
|
int written = snprintf(name, sizeof(name), "spindle.%u.%s", index, spindle_pins[pin].suffix);
|
|
if (written < 0 || (size_t)written >= sizeof(name)) return SIM_IO_ERR_PIN;
|
|
result = register_pin(name, spindle_pins[pin].type);
|
|
if (result != SIM_IO_OK) return result;
|
|
}
|
|
}
|
|
io->pins_registered = 1;
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
static void set_bit(const char *name, uint32_t value) {
|
|
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_BIT, {.bit = value ? 1U : 0U}});
|
|
}
|
|
|
|
static void set_s32(const char *name, int32_t value) {
|
|
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_S32, {.s32 = value}});
|
|
}
|
|
|
|
static void set_float(const char *name, double value) {
|
|
(void)sim_hal_set_pin(name, (sim_hal_value){SIM_HAL_FLOAT, {.real = (float)value}});
|
|
}
|
|
|
|
static void mirror_pins(sim_io *io) {
|
|
char name[64];
|
|
uint32_t index;
|
|
sim_io_state *state = &io->state;
|
|
set_bit("motion.probe-input", state->probe.input);
|
|
set_bit("iocontrol.0.user-enable-out", state->estop);
|
|
set_bit("iocontrol.0.emc-enable-in", state->ready);
|
|
set_bit("iocontrol.0.user-request-enable", 0);
|
|
set_bit("iocontrol.0.coolant-mist", 0);
|
|
set_bit("iocontrol.0.coolant-flood", 0);
|
|
set_bit("iocontrol.0.tool-prepare", state->tool_prepare);
|
|
set_s32("iocontrol.0.tool-prep-number", state->prepared_tool);
|
|
set_s32("iocontrol.0.tool-prep-pocket", state->prepared_pocket);
|
|
set_s32("iocontrol.0.tool-prep-index", state->prepared_pocket);
|
|
set_s32("iocontrol.0.tool-number", state->current_tool);
|
|
set_bit("iocontrol.0.tool-prepared", state->tool_prepared);
|
|
set_bit("iocontrol.0.tool-change", state->tool_change);
|
|
set_bit("iocontrol.0.tool-changed", state->tool_changed);
|
|
set_bit("iocontrol.0.toolchanger-fault", state->toolchanger_fault);
|
|
set_bit("iocontrol.0.toolchanger-fault-ack", state->toolchanger_fault ? 0 : 1);
|
|
set_s32("iocontrol.0.toolchanger-reason", state->toolchanger_reason);
|
|
set_bit("iocontrol.0.toolchanger-faulted", state->fault);
|
|
set_bit("iocontrol.0.toolchanger-clear-fault", 0);
|
|
set_bit("iocontrol.0.emc-abort", state->tool_state == SIM_IO_TOOL_ABORTED);
|
|
set_bit("iocontrol.0.emc-abort-ack", state->tool_state != SIM_IO_TOOL_ABORTED);
|
|
set_s32("iocontrol.0.emc-reason", state->fault_reason);
|
|
set_bit("iocontrol.0.start-change", state->tool_change);
|
|
set_bit("iocontrol.0.start-change-ack", state->tool_state != SIM_IO_TOOL_CHANGING);
|
|
set_s32("iocontrol.0.state", (int32_t)state->tool_state);
|
|
for (index = 0; index < state->spindle_count; ++index) {
|
|
sim_io_spindle_state *spindle = &state->spindles[index];
|
|
snprintf(name, sizeof(name), "spindle.%u.on", index); set_bit(name, spindle->enabled && spindle->speed_rpm != 0.0);
|
|
snprintf(name, sizeof(name), "spindle.%u.forward", index); set_bit(name, spindle->forward);
|
|
snprintf(name, sizeof(name), "spindle.%u.reverse", index); set_bit(name, spindle->reverse);
|
|
snprintf(name, sizeof(name), "spindle.%u.brake", index); set_bit(name, spindle->brake);
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-out", index); set_float(name, spindle->speed_rpm);
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-out-abs", index); set_float(name, fabs(spindle->speed_rpm));
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-out-rps", index); set_float(name, spindle->speed_rpm / 60.0);
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-out-rps-abs", index); set_float(name, fabs(spindle->speed_rpm) / 60.0);
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-cmd-rps", index); set_float(name, spindle->command_rpm / 60.0);
|
|
snprintf(name, sizeof(name), "spindle.%u.inhibit", index); set_bit(name, spindle->inhibit);
|
|
snprintf(name, sizeof(name), "spindle.%u.amp-fault-in", index); set_bit(name, spindle->amp_fault);
|
|
snprintf(name, sizeof(name), "spindle.%u.orient-angle", index); set_float(name, spindle->orient_angle);
|
|
snprintf(name, sizeof(name), "spindle.%u.orient-mode", index); set_s32(name, spindle->orient_mode);
|
|
snprintf(name, sizeof(name), "spindle.%u.orient", index); set_bit(name, spindle->orient);
|
|
snprintf(name, sizeof(name), "spindle.%u.locked", index); set_bit(name, spindle->locked);
|
|
snprintf(name, sizeof(name), "spindle.%u.is-oriented", index); set_bit(name, spindle->is_oriented);
|
|
snprintf(name, sizeof(name), "spindle.%u.orient-fault", index); set_s32(name, spindle->orient_fault);
|
|
snprintf(name, sizeof(name), "spindle.%u.revs", index); set_float(name, spindle->revs);
|
|
snprintf(name, sizeof(name), "spindle.%u.speed-in", index); set_float(name, spindle->speed_rpm);
|
|
snprintf(name, sizeof(name), "spindle.%u.at-speed", index); set_bit(name, spindle->at_speed);
|
|
snprintf(name, sizeof(name), "spindle.%u.index-enable", index); set_bit(name, spindle->index_enable);
|
|
}
|
|
}
|
|
|
|
static void state_defaults(sim_io *io) {
|
|
uint32_t index;
|
|
sim_io_state old = io->state;
|
|
memset(&io->state, 0, sizeof(io->state));
|
|
io->state.spindle_count = old.spindle_count;
|
|
io->state.tool_capacity = old.tool_capacity;
|
|
io->state.ready = 1;
|
|
io->state.current_tool = 0;
|
|
io->state.prepared_tool = -1;
|
|
io->state.prepared_pocket = -1;
|
|
io->state.toolchanger_reason = 0;
|
|
io->state.tool_state = SIM_IO_TOOL_IDLE;
|
|
io->state.last_error = SIM_IO_OK;
|
|
io->state.probe.state = SIM_IO_PROBE_IDLE;
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
io->state.spindles[index].registered = 1;
|
|
io->state.spindles[index].max_rpm = 100000.0;
|
|
io->state.spindles[index].accel_rpm_s = old.spindles[index].accel_rpm_s;
|
|
io->state.spindles[index].tolerance_rpm = old.spindles[index].tolerance_rpm;
|
|
io->state.spindles[index].at_speed = 1;
|
|
}
|
|
}
|
|
|
|
int sim_io_init(sim_io *io, uint32_t spindle_count, uint32_t tool_capacity,
|
|
double cycle_time, double spindle_accel_rpm_s,
|
|
double spindle_tolerance_rpm) {
|
|
uint32_t index;
|
|
int result;
|
|
if (io == 0) return SIM_IO_ERR_ARGUMENT;
|
|
if (spindle_count == 0 || spindle_count > SIM_IO_MAX_SPINDLES ||
|
|
tool_capacity == 0 || tool_capacity > SIM_IO_MAX_TOOLS ||
|
|
!isfinite(cycle_time) || cycle_time <= 0.0 || cycle_time > 60.0 ||
|
|
!isfinite(spindle_accel_rpm_s) || spindle_accel_rpm_s <= 0.0 ||
|
|
!isfinite(spindle_tolerance_rpm) || spindle_tolerance_rpm < 0.0)
|
|
return SIM_IO_ERR_RANGE;
|
|
memset(io, 0, sizeof(*io));
|
|
io->cycle_time = cycle_time;
|
|
io->state.spindle_count = spindle_count;
|
|
io->state.tool_capacity = tool_capacity;
|
|
for (index = 0; index < spindle_count; ++index) {
|
|
io->state.spindles[index].accel_rpm_s = spindle_accel_rpm_s;
|
|
io->state.spindles[index].tolerance_rpm = spindle_tolerance_rpm;
|
|
}
|
|
result = register_io_pins(io);
|
|
if (result != SIM_IO_OK) return result;
|
|
state_defaults(io);
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_reset(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
state_defaults(io);
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_register_pins(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
return register_io_pins(io);
|
|
}
|
|
|
|
int sim_io_spindle_command(sim_io *io, uint32_t spindle, double rpm, uint32_t enabled) {
|
|
sim_io_spindle_state *state;
|
|
if (!valid_spindle(io, spindle) || enabled > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
if (!isfinite(rpm)) { set_last(io, SIM_IO_ERR_NONFINITE); return SIM_IO_ERR_NONFINITE; }
|
|
if (fabs(rpm) > io->state.spindles[spindle].max_rpm) { set_last(io, SIM_IO_ERR_LIMIT); return SIM_IO_ERR_LIMIT; }
|
|
if (io->state.estop) { set_last(io, SIM_IO_ERR_ESTOP); return SIM_IO_ERR_ESTOP; }
|
|
if (!io->state.ready) { set_last(io, SIM_IO_ERR_NOT_READY); return SIM_IO_ERR_NOT_READY; }
|
|
state = &io->state.spindles[spindle];
|
|
state->command_rpm = enabled && !state->inhibit && !state->amp_fault ? rpm : 0.0;
|
|
state->enabled = enabled ? 1U : 0U;
|
|
set_last(io, SIM_IO_OK);
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_set_inhibit(sim_io *io, uint32_t spindle, uint32_t inhibit) {
|
|
if (!valid_spindle(io, spindle) || inhibit > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.spindles[spindle].inhibit = inhibit;
|
|
if (inhibit) io->state.spindles[spindle].command_rpm = 0.0;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_set_fault(sim_io *io, uint32_t spindle, uint32_t fault) {
|
|
if (!valid_spindle(io, spindle) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.spindles[spindle].amp_fault = fault;
|
|
if (fault) {
|
|
io->state.spindles[spindle].command_rpm = 0.0;
|
|
io->state.spindles[spindle].enabled = 0;
|
|
}
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_set_index_enable(sim_io *io, uint32_t spindle, uint32_t enable) {
|
|
if (!valid_spindle(io, spindle) || enable > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.spindles[spindle].index_enable = enable;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_set_feedback(sim_io *io, uint32_t spindle, double rpm) {
|
|
if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
|
|
if (!isfinite(rpm)) return SIM_IO_ERR_NONFINITE;
|
|
io->state.spindles[spindle].speed_rpm = rpm;
|
|
io->state.spindles[spindle].at_speed = fabs(rpm - io->state.spindles[spindle].command_rpm) <= io->state.spindles[spindle].tolerance_rpm;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_orient(sim_io *io, uint32_t spindle, double angle, int32_t mode) {
|
|
sim_io_spindle_state *state;
|
|
if (!valid_spindle(io, spindle)) return SIM_IO_ERR_ARGUMENT;
|
|
if (!isfinite(angle) || mode < 0 || mode > 2) return SIM_IO_ERR_RANGE;
|
|
if (io->state.estop) return SIM_IO_ERR_ESTOP;
|
|
state = &io->state.spindles[spindle];
|
|
if (fabs(state->speed_rpm) > state->tolerance_rpm || state->command_rpm != 0.0) return SIM_IO_ERR_BUSY;
|
|
state->orient = 1; state->locked = 0; state->is_oriented = 0;
|
|
state->orient_angle = angle; state->orient_mode = mode; state->orient_fault = 0;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_spindle_set_oriented(sim_io *io, uint32_t spindle, uint32_t oriented, int32_t fault) {
|
|
sim_io_spindle_state *state;
|
|
if (!valid_spindle(io, spindle) || oriented > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
state = &io->state.spindles[spindle];
|
|
if (!state->orient) return SIM_IO_ERR_STATE;
|
|
state->is_oriented = oriented;
|
|
state->orient_fault = fault;
|
|
if (fault) { state->orient = 0; state->locked = 0; }
|
|
else if (oriented) { state->orient = 0; state->locked = 1; }
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
static int spindle_stopped(const sim_io *io) {
|
|
uint32_t index;
|
|
for (index = 0; index < io->state.spindle_count; ++index)
|
|
if (fabs(io->state.spindles[index].speed_rpm) > io->state.spindles[index].tolerance_rpm || io->state.spindles[index].command_rpm != 0.0) return 0;
|
|
return 1;
|
|
}
|
|
|
|
int sim_io_tool_prepare(sim_io *io, int32_t tool, int32_t pocket) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (tool < 0 || pocket <= 0 || (uint32_t)pocket >= io->state.tool_capacity) return SIM_IO_ERR_RANGE;
|
|
if (io->state.estop) return SIM_IO_ERR_ESTOP;
|
|
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
|
|
if (io->state.fault) return SIM_IO_ERR_FAULT;
|
|
io->state.prepared_tool = tool; io->state.prepared_pocket = pocket;
|
|
io->state.tool_prepare = 1; io->state.tool_prepared = 0; io->state.tool_changed = 0;
|
|
io->state.tool_state = SIM_IO_TOOL_PREPARING;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_set_prepared(sim_io *io, uint32_t prepared) {
|
|
if (!valid_io(io) || prepared > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.tool_state != SIM_IO_TOOL_PREPARING) return SIM_IO_ERR_STATE;
|
|
if (!prepared) return SIM_IO_OK;
|
|
io->state.tool_prepare = 0; io->state.tool_prepared = 1; io->state.tool_state = SIM_IO_TOOL_PREPARED;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_change(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.estop) return SIM_IO_ERR_ESTOP;
|
|
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
|
|
if (io->state.fault) return SIM_IO_ERR_FAULT;
|
|
if (io->state.tool_state != SIM_IO_TOOL_PREPARED || !io->state.tool_prepared) return SIM_IO_ERR_STATE;
|
|
if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
|
|
io->state.tool_change = 1; io->state.tool_changed = 0; io->state.tool_state = SIM_IO_TOOL_CHANGING;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_set_changed(sim_io *io, uint32_t changed) {
|
|
if (!valid_io(io) || changed > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.tool_state != SIM_IO_TOOL_CHANGING) return SIM_IO_ERR_STATE;
|
|
if (!changed) return SIM_IO_OK;
|
|
io->state.tool_change = 0; io->state.tool_changed = 1; io->state.tool_prepared = 0;
|
|
io->state.current_tool = io->state.prepared_tool; io->state.tool_state = SIM_IO_TOOL_READY;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_abort(sim_io *io, int32_t reason) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.tool_prepare = 0; io->state.tool_change = 0; io->state.tool_prepared = 0;
|
|
io->state.tool_changed = 0; io->state.toolchanger_reason = reason;
|
|
io->state.tool_state = SIM_IO_TOOL_ABORTED;
|
|
io->state.last_error = SIM_IO_ERR_RECOVERY;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_set_fault(sim_io *io, uint32_t fault, int32_t reason) {
|
|
if (!valid_io(io) || fault > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.toolchanger_fault = fault; io->state.toolchanger_reason = reason;
|
|
io->state.fault = fault; io->state.fault_reason = reason;
|
|
if (fault) { io->state.tool_state = SIM_IO_TOOL_FAULT; io->state.tool_prepare = 0; io->state.tool_change = 0; }
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_tool_clear_fault(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
|
|
io->state.fault = 0; io->state.fault_reason = 0;
|
|
if (io->state.tool_state == SIM_IO_TOOL_FAULT || io->state.tool_state == SIM_IO_TOOL_ABORTED) io->state.tool_state = SIM_IO_TOOL_IDLE;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_probe_arm(sim_io *io, uint32_t probe_type) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.estop) return SIM_IO_ERR_ESTOP;
|
|
if (!io->state.ready) return SIM_IO_ERR_NOT_READY;
|
|
io->state.probe.probe_type = probe_type; io->state.probe.state = SIM_IO_PROBE_ARMED;
|
|
io->state.probe.tripped = 0; io->state.probe.tick = io->state.tick;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_probe_update(sim_io *io, const sim_posemath_pose *pose, uint32_t input) {
|
|
uint32_t when_clears;
|
|
if (!valid_io(io) || pose == 0) return SIM_IO_ERR_ARGUMENT;
|
|
if (!finite_pose(pose)) return SIM_IO_ERR_NONFINITE;
|
|
if (input > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.probe.input = input;
|
|
when_clears = (io->state.probe.probe_type & 2U) != 0;
|
|
if (io->state.probe.state == SIM_IO_PROBE_ARMED && ((input != 0U) ^ when_clears)) {
|
|
io->state.probe.position = *pose; io->state.probe.tripped = 1;
|
|
io->state.probe.state = SIM_IO_PROBE_TRIPPED; io->state.probe.tick = io->state.tick;
|
|
}
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_probe_disarm(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.probe.state == SIM_IO_PROBE_ARMED) io->state.probe.state = SIM_IO_PROBE_IDLE;
|
|
io->state.probe.probe_type = 0; io->state.probe.tripped = 0;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_set_estop(sim_io *io, uint32_t estop) {
|
|
uint32_t index;
|
|
if (!valid_io(io) || estop > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
io->state.estop = estop;
|
|
if (estop) {
|
|
io->state.ready = 0; io->state.tool_prepare = 0; io->state.tool_change = 0;
|
|
io->state.tool_prepared = 0; io->state.tool_changed = 0;
|
|
io->state.tool_state = SIM_IO_TOOL_ABORTED;
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
io->state.spindles[index].command_rpm = 0.0; io->state.spindles[index].enabled = 0;
|
|
io->state.spindles[index].brake = 1;
|
|
}
|
|
io->state.last_error = SIM_IO_ERR_ESTOP;
|
|
}
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_set_power(sim_io *io, uint32_t active) {
|
|
if (!valid_io(io) || active > 1U) return SIM_IO_ERR_ARGUMENT;
|
|
if (!active) {
|
|
io->state.ready = 0;
|
|
io->state.tool_prepare = 0;
|
|
io->state.tool_change = 0;
|
|
io->state.tool_prepared = 0;
|
|
io->state.tool_changed = 0;
|
|
io->state.tool_state = SIM_IO_TOOL_ABORTED;
|
|
for (uint32_t index = 0; index < io->state.spindle_count; ++index) {
|
|
io->state.spindles[index].command_rpm = 0.0;
|
|
io->state.spindles[index].enabled = 0;
|
|
io->state.spindles[index].brake = 1;
|
|
}
|
|
} else {
|
|
if (io->state.estop) { set_last(io, SIM_IO_ERR_ESTOP); return SIM_IO_ERR_ESTOP; }
|
|
if (io->state.fault || io->state.toolchanger_fault) { set_last(io, SIM_IO_ERR_FAULT); return SIM_IO_ERR_FAULT; }
|
|
if (!spindle_stopped(io)) { set_last(io, SIM_IO_ERR_BUSY); return SIM_IO_ERR_BUSY; }
|
|
io->state.ready = 1;
|
|
io->state.recovery_epoch += 1;
|
|
io->state.tool_state = SIM_IO_TOOL_IDLE;
|
|
}
|
|
set_last(io, SIM_IO_OK);
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_recover(sim_io *io) {
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (io->state.estop) return SIM_IO_ERR_ESTOP;
|
|
if (io->state.fault || io->state.toolchanger_fault) return SIM_IO_ERR_FAULT;
|
|
if (!spindle_stopped(io)) return SIM_IO_ERR_BUSY;
|
|
io->state.ready = 1; io->state.recovery_epoch += 1; io->state.tool_state = SIM_IO_TOOL_IDLE;
|
|
io->state.last_error = SIM_IO_OK;
|
|
mirror_pins(io);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_step(sim_io *io, uint32_t ticks) {
|
|
uint32_t tick, index;
|
|
if (!valid_io(io)) return SIM_IO_ERR_ARGUMENT;
|
|
if (ticks == 0) return SIM_IO_OK;
|
|
if (ticks > 1000000U || UINT32_MAX - io->state.tick < ticks) return SIM_IO_ERR_RANGE;
|
|
if (sim_hal_step(ticks) != SIM_HAL_OK) return SIM_IO_ERR_PIN;
|
|
for (tick = 0; tick < ticks; ++tick) {
|
|
io->state.tick += 1;
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
sim_io_spindle_state *spindle = &io->state.spindles[index];
|
|
double target = spindle->command_rpm;
|
|
double delta = spindle->accel_rpm_s * io->cycle_time;
|
|
if (spindle->amp_fault || spindle->inhibit || !io->state.ready || io->state.estop) target = 0.0;
|
|
if (spindle->speed_rpm < target) spindle->speed_rpm = fmin(target, spindle->speed_rpm + delta);
|
|
else if (spindle->speed_rpm > target) spindle->speed_rpm = fmax(target, spindle->speed_rpm - delta);
|
|
spindle->forward = spindle->speed_rpm > spindle->tolerance_rpm;
|
|
spindle->reverse = spindle->speed_rpm < -spindle->tolerance_rpm;
|
|
spindle->brake = fabs(target - spindle->speed_rpm) > spindle->tolerance_rpm && target == 0.0;
|
|
spindle->at_speed = fabs(spindle->speed_rpm - target) <= spindle->tolerance_rpm;
|
|
spindle->revs += spindle->speed_rpm * io->cycle_time / 60.0;
|
|
spindle->tick = io->state.tick;
|
|
if (spindle->index_enable && floor(spindle->revs) != floor(spindle->revs - spindle->speed_rpm * io->cycle_time / 60.0)) {
|
|
spindle->index_enable = 0; spindle->revs = spindle->revs - floor(spindle->revs);
|
|
}
|
|
if (spindle->orient && spindle->at_speed && spindle->speed_rpm == 0.0) {
|
|
spindle->orient = 0; spindle->locked = 1; spindle->is_oriented = 1;
|
|
}
|
|
}
|
|
}
|
|
mirror_pins(io);
|
|
set_last(io, SIM_IO_OK);
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
int sim_io_read_state(const sim_io *io, sim_io_state *state) {
|
|
if (!valid_io(io) || state == 0) return SIM_IO_ERR_ARGUMENT;
|
|
*state = io->state;
|
|
return SIM_IO_OK;
|
|
}
|
|
|
|
uint64_t sim_io_state_hash(const sim_io *io) {
|
|
unsigned char bytes[8192];
|
|
size_t offset = 0;
|
|
uint32_t index;
|
|
if (!valid_io(io)) return 0;
|
|
#define APPEND(value) do { memcpy(bytes + offset, &(value), sizeof(value)); offset += sizeof(value); } while (0)
|
|
APPEND(io->state.ready); APPEND(io->state.estop); APPEND(io->state.fault); APPEND(io->state.fault_reason);
|
|
APPEND(io->state.tick); APPEND(io->state.recovery_epoch); APPEND(io->state.spindle_count); APPEND(io->state.tool_capacity);
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
sim_io_spindle_state spindle = io->state.spindles[index];
|
|
spindle.command_rpm = normalized(spindle.command_rpm); spindle.speed_rpm = normalized(spindle.speed_rpm);
|
|
spindle.max_rpm = normalized(spindle.max_rpm); spindle.accel_rpm_s = normalized(spindle.accel_rpm_s);
|
|
spindle.tolerance_rpm = normalized(spindle.tolerance_rpm); spindle.orient_angle = normalized(spindle.orient_angle);
|
|
spindle.revs = normalized(spindle.revs); APPEND(spindle);
|
|
}
|
|
APPEND(io->state.tool_state); APPEND(io->state.current_tool); APPEND(io->state.prepared_tool); APPEND(io->state.prepared_pocket);
|
|
APPEND(io->state.tool_prepare); APPEND(io->state.tool_prepared); APPEND(io->state.tool_change); APPEND(io->state.tool_changed);
|
|
APPEND(io->state.toolchanger_fault); APPEND(io->state.toolchanger_reason); APPEND(io->state.probe.input);
|
|
APPEND(io->state.probe.probe_type); APPEND(io->state.probe.state); APPEND(io->state.probe.tripped); APPEND(io->state.probe.tick);
|
|
{ sim_posemath_pose pose = io->state.probe.position; double *values = &pose.tran.x; for (index = 0; index < 9; ++index) values[index] = normalized(values[index]); APPEND(pose); }
|
|
APPEND(io->state.last_error);
|
|
#undef APPEND
|
|
return io_fnv1a64(bytes, offset);
|
|
}
|
|
|
|
uint32_t sim_io_canonical(const sim_io *io, char *output, uint32_t capacity) {
|
|
char discard[1];
|
|
char *destination = output == 0 ? discard : output;
|
|
uint32_t destination_capacity = output == 0 ? 1 : capacity;
|
|
int written;
|
|
uint32_t index;
|
|
if (!valid_io(io)) return 0;
|
|
written = snprintf(destination, destination_capacity,
|
|
"{\"estop\":%u,\"fault\":%u,\"fault_reason\":%d,\"last_error\":%d,\"probe\":{\"input\":%u,\"position\":[%.9g,%.9g,%.9g],\"state\":%u,\"tick\":%u,\"tripped\":%u},\"ready\":%u,\"recovery_epoch\":%u,\"spindles\":[",
|
|
io->state.estop, io->state.fault, io->state.fault_reason, io->state.last_error,
|
|
io->state.probe.input, io->state.probe.position.tran.x, io->state.probe.position.tran.y,
|
|
io->state.probe.position.tran.z, io->state.probe.state, io->state.probe.tick,
|
|
io->state.probe.tripped, io->state.ready, io->state.recovery_epoch);
|
|
if (written < 0) return 0;
|
|
for (index = 0; index < io->state.spindle_count; ++index) {
|
|
const sim_io_spindle_state *spindle = &io->state.spindles[index];
|
|
int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
|
|
output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
|
|
"%s{\"at_speed\":%u,\"brake\":%u,\"command_rpm\":%.9g,\"forward\":%u,\"reverse\":%u,\"speed_rpm\":%.9g,\"tick\":%u}",
|
|
index ? "," : "", spindle->at_speed, spindle->brake, spindle->command_rpm,
|
|
spindle->forward, spindle->reverse, spindle->speed_rpm, spindle->tick);
|
|
if (part < 0) return 0; written += part;
|
|
}
|
|
{ int part = snprintf(destination + (output && (uint32_t)written < capacity ? written : 0),
|
|
output && (uint32_t)written < capacity ? capacity - (uint32_t)written : 0,
|
|
"],\"tick\":%u,\"tool\":{\"changed\":%u,\"current\":%d,\"prepared\":%u,\"prepared_pocket\":%d,\"prepared_tool\":%d,\"prepare\":%u,\"state\":%u}}",
|
|
io->state.tick, io->state.tool_changed, io->state.current_tool, io->state.tool_prepared,
|
|
io->state.prepared_pocket, io->state.prepared_tool, io->state.tool_prepare, io->state.tool_state);
|
|
if (part < 0) return 0; written += part; }
|
|
return (uint32_t)written + 1;
|
|
}
|