220 lines
11 KiB
JavaScript
220 lines
11 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
|
|
export const IO_CODES = Object.freeze({
|
|
ok: "sim_io_ok",
|
|
argument: "sim_io_argument",
|
|
range: "sim_io_range",
|
|
state: "sim_io_state",
|
|
not_ready: "sim_io_not_ready",
|
|
estop: "sim_io_estop",
|
|
fault: "sim_io_fault",
|
|
limit: "sim_io_limit",
|
|
nonfinite: "sim_io_nonfinite",
|
|
busy: "sim_io_busy",
|
|
probe: "sim_io_probe",
|
|
timeout: "sim_io_timeout",
|
|
recovery: "sim_io_recovery",
|
|
});
|
|
|
|
export const DEVICE_COMMANDS = Object.freeze(new Set([
|
|
"SPINDLE_COMMAND", "SPINDLE_FAULT", "SPINDLE_INHIBIT", "SPINDLE_ORIENT", "SPINDLE_ORIENTED", "IO_STEP",
|
|
"TOOL_PREPARE", "TOOL_PREPARED", "TOOL_CHANGE", "TOOL_CHANGED", "TOOL_ABORT", "TOOL_FAULT", "TOOL_CLEAR_FAULT",
|
|
"PROBE_CONFIG", "LIMIT_SET", "ESTOP", "IO_RECOVER",
|
|
]));
|
|
|
|
function clone(value) {
|
|
return structuredClone(value);
|
|
}
|
|
|
|
function finite(value) {
|
|
return typeof value === "number" && Number.isFinite(value);
|
|
}
|
|
|
|
function spindleState() {
|
|
return {
|
|
command_rpm: 0,
|
|
speed_rpm: 0,
|
|
at_speed: true,
|
|
enabled: false,
|
|
inhibit: false,
|
|
amp_fault: false,
|
|
brake: false,
|
|
orient: false,
|
|
locked: false,
|
|
is_oriented: false,
|
|
orient_angle: 0,
|
|
orient_mode: 0,
|
|
orient_fault: 0,
|
|
index_enable: false,
|
|
revs: 0,
|
|
};
|
|
}
|
|
|
|
function defaultState(spindleCount, toolCapacity) {
|
|
return {
|
|
ready: true,
|
|
estop: false,
|
|
fault: false,
|
|
fault_reason: 0,
|
|
tick: 0,
|
|
recovery_epoch: 0,
|
|
spindle_count: spindleCount,
|
|
tool_capacity: toolCapacity,
|
|
spindles: Array.from({ length: spindleCount }, spindleState),
|
|
tool: { state: "idle", current: 0, prepared: null, pocket: null, prepare: false, prepared_ack: false, changed: false, fault: false, reason: 0 },
|
|
probe: { state: "idle", type: 0, input: false, tripped: false, tick: 0, position: null },
|
|
limits: {},
|
|
last_error: IO_CODES.ok,
|
|
};
|
|
}
|
|
|
|
export class VirtualDevices {
|
|
constructor({ spindleCount = 1, toolCapacity = 1001, state = null } = {}) {
|
|
if (!Number.isSafeInteger(spindleCount) || spindleCount < 1 || spindleCount > 8) throw new Error("sim_io_spindle_capacity");
|
|
if (!Number.isSafeInteger(toolCapacity) || toolCapacity < 1 || toolCapacity > 1001) throw new Error("sim_io_tool_capacity");
|
|
this.state = state ? clone(state) : defaultState(spindleCount, toolCapacity);
|
|
}
|
|
|
|
export() { return clone(this.state); }
|
|
hash() { return fnv1a64(this.state); }
|
|
result(code = IO_CODES.ok, extra = {}) {
|
|
this.state.last_error = code;
|
|
return { status: code === IO_CODES.ok ? "verified" : "blocked", code, state_hash: this.hash(), state: this.export(), ...extra };
|
|
}
|
|
|
|
spindle(payload, action = "command") {
|
|
const index = payload?.spindle ?? 0;
|
|
const spindle = this.state.spindles[index];
|
|
if (!spindle) return this.result(IO_CODES.argument);
|
|
if (action === "command") {
|
|
const rpm = payload?.rpm;
|
|
if (!finite(rpm) || typeof payload?.enabled !== "boolean") return this.result(IO_CODES.nonfinite);
|
|
if (Math.abs(rpm) > 100000) return this.result(IO_CODES.limit);
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (!this.state.ready) return this.result(IO_CODES.not_ready);
|
|
spindle.command_rpm = payload.enabled && !spindle.inhibit && !spindle.amp_fault ? rpm : 0;
|
|
spindle.enabled = payload.enabled;
|
|
} else if (action === "fault" || action === "inhibit") {
|
|
if (typeof payload?.value !== "boolean") return this.result(IO_CODES.argument);
|
|
spindle[action === "fault" ? "amp_fault" : "inhibit"] = payload.value;
|
|
if (payload.value) spindle.command_rpm = 0;
|
|
} else if (action === "orient") {
|
|
if (!finite(payload?.angle) || !Number.isSafeInteger(payload?.mode) || payload.mode < 0 || payload.mode > 2) return this.result(IO_CODES.range);
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (Math.abs(spindle.speed_rpm) > 5 || spindle.command_rpm !== 0) return this.result(IO_CODES.busy);
|
|
spindle.orient = true; spindle.locked = false; spindle.is_oriented = false; spindle.orient_angle = payload.angle; spindle.orient_mode = payload.mode; spindle.orient_fault = 0;
|
|
} else if (action === "oriented") {
|
|
if (typeof payload?.oriented !== "boolean") return this.result(IO_CODES.argument);
|
|
if (!spindle.orient) return this.result(IO_CODES.state);
|
|
spindle.is_oriented = payload.oriented;
|
|
spindle.orient_fault = payload.fault ?? 0;
|
|
if (payload.fault) { spindle.orient = false; spindle.locked = false; }
|
|
else if (payload.oriented) { spindle.orient = false; spindle.locked = true; }
|
|
}
|
|
return this.result();
|
|
}
|
|
|
|
step(ticks) {
|
|
if (!Number.isSafeInteger(ticks) || ticks < 0 || ticks > 1000000) return this.result(IO_CODES.range);
|
|
for (let tick = 0; tick < ticks; tick += 1) {
|
|
this.state.tick += 1;
|
|
for (const spindle of this.state.spindles) {
|
|
const target = this.state.ready && !this.state.estop && !spindle.amp_fault && !spindle.inhibit ? spindle.command_rpm : 0;
|
|
const delta = 60;
|
|
if (spindle.speed_rpm < target) spindle.speed_rpm = Math.min(target, spindle.speed_rpm + delta);
|
|
else if (spindle.speed_rpm > target) spindle.speed_rpm = Math.max(target, spindle.speed_rpm - delta);
|
|
spindle.at_speed = Math.abs(spindle.speed_rpm - target) <= 5;
|
|
spindle.brake = target === 0 && Math.abs(spindle.speed_rpm) > 5;
|
|
spindle.revs += spindle.speed_rpm * 0.1 / 60;
|
|
if (spindle.orient && spindle.at_speed && spindle.speed_rpm === 0) { spindle.orient = false; spindle.locked = true; spindle.is_oriented = true; }
|
|
}
|
|
}
|
|
return this.result(IO_CODES.ok, { ticks });
|
|
}
|
|
|
|
toolAction(kind, payload = {}) {
|
|
const tool = this.state.tool;
|
|
if (kind === "TOOL_PREPARE") {
|
|
if (!Number.isSafeInteger(payload.tool) || !Number.isSafeInteger(payload.pocket) || payload.tool < 0 || payload.pocket <= 0 || payload.pocket >= this.state.tool_capacity) return this.result(IO_CODES.range);
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (!this.state.ready) return this.result(IO_CODES.not_ready);
|
|
if (this.state.fault) return this.result(IO_CODES.fault);
|
|
Object.assign(tool, { state: "preparing", prepared: payload.tool, pocket: payload.pocket, prepare: true, prepared_ack: false, changed: false });
|
|
} else if (kind === "TOOL_PREPARED") {
|
|
if (tool.state !== "preparing") return this.result(IO_CODES.state);
|
|
if (payload.prepared !== true) return this.result();
|
|
Object.assign(tool, { state: "prepared", prepare: false, prepared_ack: true });
|
|
} else if (kind === "TOOL_CHANGE") {
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (!this.state.ready) return this.result(IO_CODES.not_ready);
|
|
if (this.state.fault) return this.result(IO_CODES.fault);
|
|
if (tool.state !== "prepared" || !tool.prepared_ack) return this.result(IO_CODES.state);
|
|
if (this.state.spindles.some((spindle) => Math.abs(spindle.speed_rpm) > 5 || spindle.command_rpm !== 0)) return this.result(IO_CODES.busy);
|
|
Object.assign(tool, { state: "changing", changed: false });
|
|
} else if (kind === "TOOL_CHANGED") {
|
|
if (tool.state !== "changing") return this.result(IO_CODES.state);
|
|
if (payload.changed !== true) return this.result();
|
|
Object.assign(tool, { state: "ready", current: tool.prepared, changed: true, prepared_ack: false });
|
|
} else if (kind === "TOOL_ABORT") {
|
|
Object.assign(tool, { state: "aborted", prepare: false, prepared_ack: false, changed: false });
|
|
this.state.last_error = IO_CODES.recovery;
|
|
} else if (kind === "TOOL_FAULT") {
|
|
if (typeof payload.fault !== "boolean") return this.result(IO_CODES.argument);
|
|
tool.fault = payload.fault; tool.reason = payload.reason ?? 0; this.state.fault = payload.fault; this.state.fault_reason = tool.reason;
|
|
if (payload.fault) Object.assign(tool, { state: "fault", prepare: false, prepared_ack: false, changed: false });
|
|
} else if (kind === "TOOL_CLEAR_FAULT") {
|
|
if (tool.fault) return this.result(IO_CODES.fault);
|
|
this.state.fault = false; this.state.fault_reason = 0;
|
|
if (tool.state === "fault" || tool.state === "aborted") tool.state = "idle";
|
|
}
|
|
return this.result();
|
|
}
|
|
|
|
probeAction(payload = {}) {
|
|
const probe = this.state.probe;
|
|
if (payload.action === "arm") {
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (!this.state.ready) return this.result(IO_CODES.not_ready);
|
|
Object.assign(probe, { state: "armed", type: payload.probe_type ?? 0, input: false, tripped: false, tick: this.state.tick, position: null });
|
|
} else if (payload.action === "update") {
|
|
const position = payload.position;
|
|
if (!position || !Object.values(position).every(finite)) return this.result(IO_CODES.nonfinite);
|
|
probe.input = Boolean(payload.input);
|
|
if (probe.state === "armed" && probe.input) Object.assign(probe, { state: "tripped", tripped: true, tick: this.state.tick, position: clone(position) });
|
|
} else if (payload.action === "disarm") {
|
|
Object.assign(probe, { state: "idle", type: 0, tripped: false });
|
|
} else return this.result(IO_CODES.argument);
|
|
return this.result();
|
|
}
|
|
|
|
command(kind, payload = {}) {
|
|
if (kind === "SPINDLE_COMMAND") return this.spindle(payload, "command");
|
|
if (kind === "SPINDLE_FAULT") return this.spindle(payload, "fault");
|
|
if (kind === "SPINDLE_INHIBIT") return this.spindle(payload, "inhibit");
|
|
if (kind === "SPINDLE_ORIENT") return this.spindle(payload, "orient");
|
|
if (kind === "SPINDLE_ORIENTED") return this.spindle(payload, "oriented");
|
|
if (kind === "IO_STEP") return this.step(payload.ticks);
|
|
if (kind.startsWith("TOOL_")) return this.toolAction(kind, payload);
|
|
if (kind === "PROBE_CONFIG") return this.probeAction(payload);
|
|
if (kind === "LIMIT_SET") {
|
|
if (typeof payload.name !== "string" || typeof payload.active !== "boolean") return this.result(IO_CODES.argument);
|
|
this.state.limits[payload.name] = payload.active; return this.result();
|
|
}
|
|
if (kind === "ESTOP") {
|
|
if (typeof payload.active !== "boolean") return this.result(IO_CODES.argument);
|
|
this.state.estop = payload.active;
|
|
if (payload.active) { this.state.ready = false; this.state.tool.state = "aborted"; for (const spindle of this.state.spindles) { spindle.command_rpm = 0; spindle.enabled = false; spindle.brake = true; } return this.result(IO_CODES.estop); }
|
|
return this.result();
|
|
}
|
|
if (kind === "IO_RECOVER") {
|
|
if (this.state.estop) return this.result(IO_CODES.estop);
|
|
if (this.state.fault || this.state.tool.fault) return this.result(IO_CODES.fault);
|
|
if (this.state.spindles.some((spindle) => Math.abs(spindle.speed_rpm) > 5 || spindle.command_rpm !== 0)) return this.result(IO_CODES.busy);
|
|
this.state.ready = true; this.state.recovery_epoch += 1; this.state.tool.state = "idle"; return this.result();
|
|
}
|
|
return this.result(IO_CODES.argument);
|
|
}
|
|
|
|
applyEvent(kind, payload) { return this.command(kind, payload); }
|
|
}
|