zach/dotter

// Pen-lift servo handling and XYZ position bookkeeping (port of

// nextdrawcore/pen_handling.py).

import { exhaustQueue, QG_PEN_UP, readStatusByte } from './control.js';

import { pyInt, pyRound } from './calc.js';

export function newPenPosition() {

  return { xpos: 0, ypos: 0, zUp: null, accum1: 0, accum2: 0, homed: false };

}

/**

 * Servo travel time (ms) between pen-up and pen-down heights: a 4th-power

 * blend of the fast-servo transit time and the rate-limited sweep time.

 */

export function penLiftTimes(nd, penDownPos) {

  const p = nd.params;

  const o = nd.options;

  const vDist = Math.abs(o.penPosUp - penDownPos);

  const timeFor = (rate, extraDelay) => {

    let t = pyInt(((p.servoMoveSlope * vDist + p.servoMoveMin) ** 4 +

      (p.servoSweepTime * vDist / rate) ** 4) ** 0.25);

    if (vDist < 0.9) t = 0; // Same up and down positions: no delay

    return Math.max(0, t + extraDelay);

  };

  return { raiseTime: timeFor(o.penRateRaise, p.penDelayUp), lowerTime: timeFor(o.penRateLower, p.penDelayDown) };

}

/** Servo PWM values and rates sent to the EBB for the current settings. */

export function servoSettings(nd, penPosDown) {

  const p = nd.params;

  const o = nd.options;

  // Units of "ms / 100", since the rates are percentages.

  const pwmPeriod = p.zMotor ? 0.03 : 0.24; // 1 channel vs 8 channels at 3 ms

  const range = p.servoMax - p.servoMin;

  const slope = range / 100.0;

  const rateScale = range * pwmPeriod / p.servoSweepTime;

  return {

    rateUp: pyRound(rateScale * o.penRateRaise),

    rateDown: pyRound(rateScale * o.penRateLower),

    posUp: pyRound(p.servoMin + slope * o.penPosUp),

    posDown: pyRound(p.servoMin + slope * penPosDown),

  };

}

export class PenHandler {

  constructor() {

    this.penPosDown = null; // Active pen-down height (may be a temporary override)

    this.useTempHeight = false;

    this.times = { raiseTime: null, lowerTime: null };

    this.lifts = 0;

    // [penPosUp, penPosDown, zMotor, penUp] as read from / desired on the EBB.

    this.initState = [-1, -1, -1, null];

    this.initGoal = [-1, -1, -1, null];

    this.phys = newPenPosition(); // Physical pen position

    this.turtle = newPenPosition(); // Commanded ("turtle") position, interactive API

  }

  reset() {

    this.lifts = 0;

    this.useTempHeight = false;

  }

  updateHeights(nd) {

    if (!this.useTempHeight) this.penPosDown = nd.options.penPosDown;

    this.times = penLiftTimes(nd, this.penPosDown);

    this.initGoal[0] = nd.options.penPosUp;

    this.initGoal[1] = this.penPosDown;

  }

  async raise(nd) {

    if (this.phys.zUp) return;

    this.lifts += 1;

    const vTime = this.times.raiseTime;

    if (nd.options.preview) {

      nd.plotStatus.stats.ptEstimate += vTime;

    } else {

      await nd.machine.penRaise(vTime, nd.params.servoPin);

      if (vTime > 50 && !['utility', 'align', 'cycle'].includes(nd.options.mode)) {

        await nd.clock.sleep(vTime - 30);

      }

      if (nd.params.useB3Out) {

        if (nd.params.syncB3) await exhaustQueue(nd);

        await nd.machine.dioBSet(3, 0);

      }

    }

    this.phys.zUp = true;

  }

  async lower(nd) {

    if (this.phys.zUp != null && !this.phys.zUp) return; // Known to be down already

    if (nd.plotStatus.stopped) return;

    const vTime = this.times.lowerTime;

    if (nd.options.preview) {

      nd.plotStatus.stats.ptEstimate += vTime;

    } else {

      await nd.machine.penLower(vTime, nd.params.servoPin);

      if (vTime > 50 && !['utility', 'align', 'cycle'].includes(nd.options.mode)) {

        await nd.clock.sleep(vTime - 30);

      }

      if (nd.params.useB3Out) {

        if (nd.params.syncB3) await exhaustQueue(nd);

        await nd.machine.dioBSet(3, 1);

      }

    }

    this.phys.zUp = false;

  }

  /** Lower, pause 500 ms, raise. Setup utility only. */

  async cycle(nd) {

    await this.lower(nd);

    await nd.machine.timedPause(500);

    await this.raise(nd);

  }

  /**

   * Read back the pen configuration cached in EBB variables 10 and 11 (and

   * the QG pen bit) to tell whether the servo is already set up as we want.

   */

  async findPenState(nd) {

    const codeUp = await nd.machine.varRead(10);

    const codeDown = await nd.machine.varRead(11);

    if (codeUp == null || codeDown == null) {

      this.initState[0] = -1;

      this.initState[1] = -1;

      return;

    }

    if (nd.params.zMotor) {

      if (codeUp > 101 && codeDown > 101) this.initState[2] = 1;

    } else if (codeUp < 102 && codeDown < 102) {

      this.initState[2] = 0;

    }

    const decode = (code) => (code === 0 ? -1 : code > 101 ? code - 102 : code - 1);

    this.initState[0] = decode(codeUp);

    this.initState[1] = decode(codeDown);

    const qg = await readStatusByte(nd);

    this.initState[3] = qg == null ? true : Boolean(qg & QG_PEN_UP);

  }

  /**

   * Configure servo positions/rates on the EBB and put the pen into a known

   * state (normally up). Skips the physical move if the EBB reports it is

   * already configured identically.

   */

  async servoInit(nd) {

    this.updateHeights(nd);

    if (nd.options.preview) this.phys.zUp = true;

    if (nd.options.preview || !nd.machine.connected) return;

    let codeUp = 1 + nd.options.penPosUp; // 1-101

    let codeDown = 1 + this.penPosDown;

    this.initGoal[0] = nd.options.penPosUp;

    this.initGoal[1] = this.penPosDown;

    if (nd.params.zMotor) {

      this.initGoal[2] = 1;

      codeUp += 101; // 102-202 marks the brushless motor

      codeDown += 101;

    } else {

      this.initGoal[2] = 0;

    }

    await this.findPenState(nd);

    const initialized = this.initState[0] === this.initGoal[0] &&

      this.initState[1] === this.initGoal[1] && this.initState[2] === this.initGoal[2];

    if (initialized) {

      this.phys.zUp = this.initState[3];

      this.initGoal[3] = this.initState[3];

    }

    const o = nd.options;

    if ((o.mode === 'utility' && o.utilityCmd === 'lower_pen') ||

        (o.mode === 'toggle' && Boolean(this.phys.zUp)) || o.mode === 'cycle') {

      this.initGoal[3] = false;

    } else if (!initialized || o.mode === 'toggle' ||

        (o.mode === 'utility' && o.utilityCmd === 'raise_pen')) {

      this.initGoal[3] = true;

    }

    const s = servoSettings(nd, this.penPosDown);

    const pin = nd.params.servoPin;

    await nd.machine.penRateUp(s.rateUp);

    await nd.machine.penRateDown(s.rateDown);

    if (nd.params.useB3Out) await nd.machine.dioBConfig(3, 0, 0);

    await nd.machine.penPosUp(s.posUp);

    await nd.machine.penPosDown(s.posDown);

    if (initialized && this.phys.zUp != null && this.phys.zUp === this.initGoal[3]) return;

    if (!this.initGoal[3]) {

      await nd.machine.penLower(this.times.lowerTime, pin);

      if (nd.params.useB3Out) await nd.machine.dioBSet(3, 1);

      this.phys.zUp = false;

    }

    if (this.initGoal[3]) {

      await nd.machine.penRaise(this.times.raiseTime, pin);

      if (nd.params.useB3Out) await nd.machine.dioBSet(3, 0);

      this.phys.zUp = true;

    }

    if (nd.params.zMotor) {

      await nd.machine.command('SC,8,1'); // 1 channel of servo PWM

    } else {

      await nd.machine.command('SC,8,8');

      await nd.machine.servoTimeout(nd.params.servoTimeout, null);

    }

    this.initState = [...this.initGoal];

    await nd.machine.varWrite(codeUp, 10);

    await nd.machine.varWrite(codeDown, 11);

  }

}