linuxcncWebWASM/web/worker/motion-state.mjs

import { fnv1a64 } from "./replay.mjs";

export const MOTION_SCHEMA = "sim_motion.v1";
const AXES = Object.freeze(["x", "y", "z"]);
const MAX_POSITION = 1e9;
const MAX_SPEED = 1e6;
const CYCLE_DISTANCE = 0.001;

function clone(value) { return structuredClone(value); }
function finite(value) { return typeof value === "number" && Number.isFinite(value); }
function axis(value) { return typeof value === "string" && AXES.includes(value.toLowerCase()) ? value.toLowerCase() : null; }

export class MotionState {
  constructor() { this.reset(); }

  reset() {
    this.state = {
      schema: MOTION_SCHEMA,
      tick: 0,
      axes: { x: 0, y: 0, z: 0 },
      joints: [0, 0, 0],
      velocity: 0,
      feed_override: 1,
      spindle_override: 1,
      homed: false,
      homed_axes: [false, false, false],
      limits: { x: false, y: false, z: false },
      following_error: 0,
      jog: null,
      estop: false,
      fault: null,
    };
    return this.export();
  }

  export() { const value = clone(this.state); return { ...value, state_hash: fnv1a64(value) }; }
  hash() { return this.export().state_hash; }

  fail(code, detail = null) { return { status: "error", code, ...(detail ? { detail } : {}), state: this.export() }; }
  ok(command) { return { status: "verified", command, state: this.export() }; }

  home(target = null) {
    if (this.state.estop) return this.fail("motion_estop_active");
    if (target !== null && !axis(target)) return this.fail("motion_axis_invalid");
    const targets = target === null ? AXES : [axis(target)];
    for (const name of targets) {
      const index = AXES.indexOf(name);
      this.state.axes[name] = 0;
      this.state.joints[index] = 0;
      this.state.homed_axes[index] = true;
      this.state.limits[name] = false;
    }
    this.state.homed = this.state.homed_axes.every(Boolean);
    this.state.fault = null;
    this.state.following_error = 0;
    this.state.tick += 1;
    return this.ok("HOME");
  }

  setOverride(feed, spindle = this.state.spindle_override) {
    if (!finite(feed) || feed < 0 || feed > 2 || !finite(spindle) || spindle < 0 || spindle > 2) return this.fail("motion_override_range");
    this.state.feed_override = feed;
    this.state.spindle_override = spindle;
    return this.ok("SET_OVERRIDE");
  }

  startJog({ axis: requestedAxis, direction = 1, speed = 1, increment = null } = {}) {
    const name = axis(requestedAxis);
    if (!name) return this.fail("motion_axis_invalid");
    if (!this.state.homed) return this.fail("motion_unhomed");
    if (this.state.estop || this.state.fault) return this.fail("motion_faulted");
    if (!Number.isSafeInteger(direction) || ![-1, 1].includes(direction) || !finite(speed) || speed <= 0 || speed > MAX_SPEED) return this.fail("motion_jog_range");
    const jogIncrement = finite(increment) && increment > 0 ? Number(increment) : null;
    this.state.jog = { axis: name, direction, speed, ...(jogIncrement === null ? {} : { increment: jogIncrement }) };
    this.state.velocity = speed * this.state.feed_override;
    return this.ok("JOG_START");
  }

  stopJog() {
    this.state.jog = null;
    this.state.velocity = 0;
    return this.ok("JOG_STOP");
  }

  setLimit(requestedAxis, active) {
    const name = axis(requestedAxis);
    if (!name || typeof active !== "boolean") return this.fail("motion_limit_argument");
    this.state.limits[name] = active;
    if (active && this.state.jog?.axis === name) this.stopJog();
    if (active) this.state.fault = "limit";
    else if (this.state.fault === "limit") this.state.fault = null;
    return this.ok("LIMIT_SET");
  }

  setFollowingError(value) {
    if (!finite(value) || Math.abs(value) > MAX_POSITION) return this.fail("motion_following_error_nonfinite");
    this.state.following_error = value;
    if (Math.abs(value) > 0.01) { this.state.fault = "following_error"; this.state.jog = null; this.state.velocity = 0; }
    return this.ok("FOLLOWING_ERROR");
  }

  resetFault() {
    if (this.state.estop) return this.fail("motion_estop_active");
    if (Object.values(this.state.limits).some(Boolean)) return this.fail("motion_limit_active");
    this.state.fault = null;
    this.state.following_error = 0;
    this.state.velocity = 0;
    this.state.jog = null;
    return this.ok("RESET");
  }

  estop(active) {
    if (typeof active !== "boolean") return this.fail("motion_estop_argument");
    this.state.estop = active;
    if (active) { this.state.jog = null; this.state.velocity = 0; this.state.fault = "estop"; }
    else if (this.state.fault === "estop") this.state.fault = null;
    return this.ok("ESTOP");
  }

  step(tick) {
    if (!Number.isSafeInteger(tick) || tick < this.state.tick) return this.fail("motion_tick_regression");
    this.state.tick = tick;
    if (!this.state.jog || this.state.estop || this.state.fault) { this.state.velocity = 0; return this.export(); }
    const { axis: name, direction, speed } = this.state.jog;
    if (this.state.limits[name]) { this.state.fault = "limit"; this.state.jog = null; this.state.velocity = 0; return this.export(); }
    const requested = speed * this.state.feed_override * CYCLE_DISTANCE;
    const increment = this.state.jog.increment == null ? null : this.state.jog.increment;
    const delta = direction * (increment === null ? requested : Math.min(requested, increment));
    const next = this.state.axes[name] + delta;
    if (!finite(next) || Math.abs(next) > MAX_POSITION) { this.state.fault = "position_range"; this.state.jog = null; this.state.velocity = 0; return this.export(); }
    this.state.axes[name] = next;
    this.state.joints[AXES.indexOf(name)] = next;
    this.state.velocity = Math.abs(speed * this.state.feed_override);
    if (increment !== null) {
      this.state.jog.increment = increment - Math.abs(delta);
      if (this.state.jog.increment <= 1e-12) { this.state.jog = null; this.state.velocity = 0; }
    }
    return this.export();
  }

  applyWasmPosition(position, velocity = 0, tick = this.state.tick) {
    if (!position || ![position.x, position.y, position.z].every(finite)) return this.fail("motion_wasm_position_invalid");
    this.state.axes = { x: position.x, y: position.y, z: position.z };
    this.state.joints = [position.x, position.y, position.z];
    this.state.velocity = finite(velocity) ? velocity : 0;
    this.state.tick = Math.max(this.state.tick, Number.isSafeInteger(tick) ? tick : this.state.tick);
    return this.export();
  }

  restore(value) {
    if (!value || value.schema !== MOTION_SCHEMA) throw new Error("motion_restore_schema");
    const unsigned = { ...value }; delete unsigned.state_hash;
    if (value.state_hash !== fnv1a64(unsigned)) throw new Error("motion_restore_hash");
    if (!finite(value.tick) || !value.axes || !Array.isArray(value.joints) || value.joints.length !== 3) throw new Error("motion_restore_shape");
    const homedAxes = Array.isArray(value.homed_axes)
      ? value.homed_axes.slice(0, AXES.length).map(Boolean)
      : AXES.map(() => value.homed === true);
    if (homedAxes.length !== AXES.length) throw new Error("motion_restore_homed_shape");
    this.state = { ...clone(unsigned), homed_axes: homedAxes, homed: homedAxes.every(Boolean) };
    return this.export();
  }
}

export { AXES };