zach/dotter

// S-curve motion-planning utilities (port of nextdrawcore/plan_utils.py).

// Units are inches and seconds unless noted; "ISR units" are the EBB's

// 25 kHz accumulator units (rate 2^31 == one step per 40 µs tick).

import { cubicSolve, isClose, isComplex, moveDistT3, rateT3 } from './calc.js';

/** Pen-up and pen-down jerk (in/s^3) for the current accel setting. */

export function calcJerk(nd) {

  let accelScaled = Math.min(nd.options.accel, 100) / 100.0;

  accelScaled = accelScaled * accelScaled * accelScaled;

  return [nd.params.jerkPenUp * accelScaled, nd.params.jerkPenDown * accelScaled];

}

/**

 * Plan a 1D S-curve. With dist == null, return the distance needed to go

 * from vIn to vMax. Otherwise return the highest speed reachable from vIn

 * over dist (equivalently, the highest initial speed that can decelerate to

 * vIn), capped at vMax. Returns null on failure.

 * See plan_utils.scurve_plan for the derivation.

 */

export function scurvePlan(vIn, vMax, jM, dist = null, minTime = null) {

  if (vIn == null || vMax == null || jM == null) return null;

  vIn = Math.abs(vIn);

  vMax = Math.abs(vMax);

  jM = Math.abs(jM);

  if (jM === 0) jM = 1000; // Avoid division by zero in constant-velocity mode.

  let tM = Math.sqrt(Math.abs(vMax - vIn) / jM);

  let dMax = 2 * vIn * tM + jM * tM * tM * tM;

  if (dist == null && isClose(tM, 0)) return 0;

  if (minTime != null && tM < minTime / 2) {

    tM = minTime / 2;

    const jerkNew = Math.abs(vMax - vIn) / (tM * tM);

    dMax = 2 * vIn * tM + jerkNew * tM * tM * tM;

  }

  if (dist == null) return dMax;

  dist = Math.abs(dist);

  if (dist >= dMax || isClose(dist, dMax)) return vMax;

  // Solve jM*t^3 + 2*vIn*t - dist = 0 for the shortest positive real t.

  tM = 1e100;

  for (const item of cubicSolve(jM, 0, 2 * vIn, -dist)) {

    if (isComplex(item)) continue;

    if (item > 0 && item <= tM) tM = item;

  }

  if (tM > 1e90) return null;

  if (minTime != null && tM < minTime / 2) {

    tM = minTime / 2;

    if (vIn !== 0 && dist / vIn < minTime) {

      // Actually a deceleration; forward speed can never be < 0.

      return Math.max((dist - vIn * tM) / tM, 0);

    }

    jM = Math.abs(dist - 2 * vIn * tM) / (tM * tM * tM);

  }

  return Math.min(vMax, vIn + jM * tM * tM);

}

function scurveJerkSearch(vStart, vEnd, dist, maxJerk, tMin, leniency) {

  const vI = Math.min(vStart, vEnd);

  const vF = Math.max(vStart, vEnd);

  let tmLower = vI === 0 ? Math.max(tMin, dist / (2 * 0.001)) : Math.max(tMin, dist / (2 * vI));

  let tmUpper = Math.sqrt((vF - vI) / (maxJerk * leniency));

  let tM = tmLower + tmUpper / 10;

  for (let iteration = 0; iteration < 50; iteration++) {

    const jTemp = Math.abs(dist - 2 * vI * tM) / (tM * tM * tM);

    const vFTemp = vI + jTemp * tM * tM;

    if (isClose(vFTemp, vF, 1e-6)) return jTemp;

    if (vFTemp > vF) tmUpper = tM;

    else tmLower = tM;

    tM = (tmLower + tmUpper) / 2.0;

  }

  return null;

}

/** Lower-jerk single S-curve covering dist from vStart to vEnd, or null. */

export function scurveJerk(vStart, vEnd, dist, maxJerk) {

  return scurveJerkSearch(vStart, vEnd, dist, maxJerk, 0.003, 1.25);

}

/** As scurveJerk, more lenient in jerk and move time. */

export function scurveJerk2(vStart, vEnd, dist, maxJerk) {

  return scurveJerkSearch(vStart, vEnd, dist, maxJerk, 0.002, 1.3);

}

/** Travel time of an S-curve from vI to vF at the given jerk. */

export function scurveTime(vI, vF, jerk) {

  if (jerk === 0) return 0;

  return 2 * Math.sqrt(Math.abs((vF - vI) / jerk));

}

/**

 * Peak speed reachable in an accelerate-then-decelerate move of length dist

 * that cannot reach vMax (found by bisection).

 */

export function striangle(vI, vF, vMax, jerk, dist) {

  let lower = Math.max(vI, vF);

  let upper = vMax;

  // The Python version loops until convergence; cap iterations as a safeguard.

  for (let i = 0; i < 10000; i++) {

    const testV = (lower + upper) / 2.0;

    const testDist = scurvePlan(vI, testV, jerk, null) + scurvePlan(vF, testV, jerk, null);

    if (isClose(testDist, dist, 1e-5)) return testV;

    if (testDist > dist) upper = testV;

    else lower = testV;

  }

  return (lower + upper) / 2.0;

}

/**

 * Advance xyzPos by a TD move (two chained T3 moves in firmware).

 * Returns [steps1, steps2, inches, finalRate1, finalRate2].

 */

export function tdSegData(td, xyzPos, stepScale) {

  const [moveTime, v1a, v1b, a1, j1, v2a, v2b, a2, j2] = td;

  const stepDivisor = stepScale * 2.0;

  let s1a, s2a;

  [s1a, xyzPos.accum1] = moveDistT3(moveTime, v1a, 0, j1, xyzPos.accum1);

  [s2a, xyzPos.accum2] = moveDistT3(moveTime, v2a, 0, j2, xyzPos.accum2);

  let m1 = s1a / stepDivisor;

  let m2 = s2a / stepDivisor;

  let dx = m1 + m2;

  let dy = m1 - m2;

  let inches = Math.sqrt(dx * dx + dy * dy);

  xyzPos.xpos += dx;

  xyzPos.ypos += dy;

  let s1b, s2b;

  [s1b, xyzPos.accum1] = moveDistT3(moveTime, v1b, a1, -j1, xyzPos.accum1);

  [s2b, xyzPos.accum2] = moveDistT3(moveTime, v2b, a2, -j2, xyzPos.accum2);

  m1 = s1b / stepDivisor;

  m2 = s2b / stepDivisor;

  dx = m1 + m2;

  dy = m1 - m2;

  inches += Math.sqrt(dx * dx + dy * dy);

  xyzPos.xpos += dx;

  xyzPos.ypos += dy;

  const rate1 = rateT3(moveTime, v1b, a1, -j1);

  const rate2 = rateT3(moveTime, v2b, a2, -j2);

  return [s1a + s1b, s2a + s2b, inches, rate1, rate2];

}

/**

 * Advance xyzPos by a T3 move.

 * Returns [steps1, steps2, inches, finalRate1, finalRate2].

 */

export function t3SegData(t3, xyzPos, stepScale) {

  const [moveTime, v1, a1, j1, v2, a2, j2] = t3;

  let s1, s2;

  [s1, xyzPos.accum1] = moveDistT3(moveTime, v1, a1, j1, xyzPos.accum1);

  [s2, xyzPos.accum2] = moveDistT3(moveTime, v2, a2, j2, xyzPos.accum2);

  const m1 = s1 / (stepScale * 2.0);

  const m2 = s2 / (stepScale * 2.0);

  const dx = m1 + m2;

  const dy = m1 - m2;

  const inches = Math.sqrt(dx * dx + dy * dy);

  xyzPos.xpos += dx;

  xyzPos.ypos += dy;

  return [s1, s2, inches, rateT3(moveTime, v1, a1, j1), rateT3(moveTime, v2, a2, j2)];

}