89 lines
5.9 KiB
JavaScript
89 lines
5.9 KiB
JavaScript
import { fnv1a64 } from "./replay.mjs";
|
|
import { parseIni } from "../workspace/ini-parser.mjs";
|
|
|
|
const STATES = ["IDLE", "START", "UNLOCK", "UNLOCK_WAIT", "INITIAL_BACKOFF_START", "INITIAL_BACKOFF_WAIT", "INITIAL_SEARCH_START", "INITIAL_SEARCH_WAIT", "SET_COARSE_POSITION", "FINAL_BACKOFF_START", "FINAL_BACKOFF_WAIT", "RISE_SEARCH_START", "RISE_SEARCH_WAIT", "FALL_SEARCH_START", "FALL_SEARCH_WAIT", "SET_SWITCH_POSITION", "INDEX_ONLY_START", "INDEX_SEARCH_START", "INDEX_SEARCH_WAIT", "SET_INDEX_POSITION", "FINAL_MOVE_START", "FINAL_MOVE_WAIT", "LOCK", "LOCK_WAIT", "FINISHED", "ABORT"];
|
|
export class WasmNativeHoming {
|
|
constructor(core, configuration, kinematics) {
|
|
this.core = core;
|
|
this.kinematics = kinematics;
|
|
this.jointCount = configuration.home_limits.joint_count;
|
|
this.size = core._sim_native_home_size();
|
|
this.pointer = core._malloc(this.size);
|
|
this.buffer = core._malloc(12 * 8);
|
|
if (!this.pointer || !this.buffer) throw new Error("native_home_allocation_failed");
|
|
this.available = true;
|
|
const parsed = parseIni({ entrypoint: configuration.entrypoint, workspace: { read: (path) => {
|
|
const value = configuration.files[path.replace(/^\/workspace\//, "")];
|
|
if (typeof value !== "string") throw new Error(`home_ini_missing:${path}`);
|
|
return value;
|
|
} } });
|
|
this.parameters = Array.from({ length: this.jointCount }, (_, j) => {
|
|
const section = parsed.sections[`JOINT_${j}`] ?? {};
|
|
const n = (key, fallback) => section[key] === undefined ? fallback : Number(section[key]);
|
|
const flags = [["HOME_IGNORE_LIMITS", 1], ["HOME_USE_INDEX", 2], ["HOME_IS_SHARED", 4], ["LOCKING_INDEXER", 8], ["HOME_INDEX_NO_ENCODER_RESET", 128]].reduce((mask, [key, bit]) => mask | (n(key, 0) ? bit : 0), 0);
|
|
const encoder = n("HOME_ABSOLUTE_ENCODER", 0);
|
|
if (![0, 1, 2].includes(encoder)) throw new Error("native_home_absolute_encoder_invalid");
|
|
return [n("HOME_OFFSET", 0), n("HOME", 0), n("HOME_FINAL_VEL", -1), n("HOME_SEARCH_VEL", 0), n("HOME_LATCH_VEL", 0), flags | (encoder ? 16 | 32 | (encoder === 2 ? 64 : 0) : 0), n("HOME_SEQUENCE", 999), n("VOLATILE_HOME", 0), n("MIN_LIMIT", -1e9), n("MAX_LIMIT", 1e9), n("MAX_VELOCITY", 1), n("MAX_ACCELERATION", 1)];
|
|
});
|
|
this.period = configuration.kinematics.servo_cycle_time;
|
|
this.reset();
|
|
}
|
|
invoke(name, ...args) {
|
|
const rc = this.core[`_sim_native_home_${name}`](this.pointer, ...args);
|
|
if (rc !== 0) throw new Error(`native_home_${name}_${rc}`);
|
|
}
|
|
get(j, field) { return this.core._sim_native_home_get(this.pointer, j, field); }
|
|
get active() { return Boolean(this.get(0, 0)); }
|
|
get mode() { return [null, "FREE", "COORD", "TELEOP"][this.get(0, 1)]; }
|
|
setMode(mode) { this.invoke("mode", { FREE: 1, COORD: 2, TELEOP: 3 }[mode]); }
|
|
sync(joints) { this.core.HEAPF64.set(joints, this.buffer / 8); this.invoke("sync", this.buffer); }
|
|
home(seq, joint = null) { this.invoke("start", joint ?? -1, seq); return this.read(); }
|
|
step() { this.invoke("step"); return this.read(); }
|
|
cancel(volatile = false) { this.invoke("cancel", Number(volatile)); return this.read(); }
|
|
reset() {
|
|
this.invoke("init", this.jointCount, this.period);
|
|
this.parameters.forEach((p, j) => { this.core.HEAPF64.set(p, this.buffer / 8); this.invoke("config", j, this.buffer); });
|
|
return this.read();
|
|
}
|
|
check(joints) { if (!this.active) this.sync(joints); return this.read(); }
|
|
read() {
|
|
const joint = Array.from({ length: this.jointCount }, (_, j) => ({
|
|
joint: j, homing: Boolean(this.get(j, 4)), homed: Boolean(this.get(j, 5)),
|
|
home_state: this.get(j, 6), home_state_name: `HOME_${STATES[this.get(j, 6)]}`,
|
|
position: this.get(j, 7), motor_position: this.get(j, 8), motor_offset: this.get(j, 9),
|
|
home_sequence: this.get(j, 10), velocity: this.get(j, 11), index_enable: Boolean(this.get(j, 12)),
|
|
}));
|
|
const joints = joint.map((j) => j.position);
|
|
const result = this.kinematics.forward(joints);
|
|
return { status: result.status, ...(result.code ? { code: result.code } : {}), state: {
|
|
schema: "sim_home_limit_state.v1", source: "linuxcnc-homing.c", joint_count: this.jointCount,
|
|
homed: joint.every((j) => j.homed), homed_mask: joint.reduce((mask, j) => mask | (j.homed ? 1 << j.joint : 0), 0),
|
|
homing: this.active, motion_mode: this.mode, joint, joints, pose: result.pose ?? null,
|
|
home_sequence: this.get(0, 3), tick: this.get(0, 2), last_error: result.result ?? 0, limit_error: result.result === -702 ? -702 : 0, kinematics_error: result.result ?? 0,
|
|
} };
|
|
}
|
|
exportState() {
|
|
// emcmot_joint_t includes large unused compensation tables. Preserve the
|
|
// complete native state while avoiding repeated zero bytes in checkpoints.
|
|
const bytes = this.core.HEAPU8.subarray(this.pointer, this.pointer + this.size);
|
|
const nonzero = [];
|
|
for (let i = 0; i < bytes.length; i++) if (bytes[i] !== 0) nonzero.push([i, bytes[i]]);
|
|
const value = { schema: "native-homing.v1", parameters: this.parameters, period: this.period,
|
|
byte_length: this.size, nonzero_bytes: nonzero };
|
|
return { ...value, state_hash: fnv1a64(value) };
|
|
}
|
|
restoreState(value) {
|
|
const { state_hash, ...unsigned } = value;
|
|
if (value.schema !== "native-homing.v1" || state_hash !== fnv1a64(unsigned) || value.byte_length !== this.size || value.period !== this.period || fnv1a64(value.parameters) !== fnv1a64(this.parameters) || !Array.isArray(value.nonzero_bytes)) throw new Error("native_home_restore_invalid");
|
|
let previous = -1;
|
|
const bytes = new Uint8Array(this.size);
|
|
for (const [offset, byte] of value.nonzero_bytes) {
|
|
if (!Number.isInteger(offset) || offset <= previous || offset >= this.size || !Number.isInteger(byte) || byte < 1 || byte > 255) throw new Error("native_home_restore_bytes_invalid");
|
|
bytes[offset] = byte; previous = offset;
|
|
}
|
|
this.core.HEAPU8.set(bytes, this.pointer);
|
|
return this.read();
|
|
}
|
|
destroy() { this.core._free(this.pointer); this.core._free(this.buffer); }
|
|
}
|