@horizon36596/zenith-core 0.1.1

package/dist/sim/params.js

@@ -0,0 +1,195 @@
+import { robotLimits } from "../kinematics.js";
+/**
+ * Where every number the instant sim runs on comes from, in one place, with the order of
+ * preference stated: the caller's `options`, then `robot.json`, then a default. Each default is
+ * either Pedro Pathing v3's own `ForesightConfig` default (read from the pinned sources jar, see
+ * site/docs/simulation.md) or derived from a number `robot.json` already carries, and
+ * says which. None is a number made up for the purpose (CLAUDE.md rule 4).
+ *
+ * The follower gains, the drivetrain geometry and each condition's `ledger` meaning are optional
+ * keys in `robot.json` (`followerSchema` and `drivetrainSchema` in `packages/schema/src/robot.ts`,
+ * site/docs/file-format.md). This file still reads them structurally, so a key the schema adds
+ * later takes effect without a change here.
+ */
+/** The default tick for the preview, in seconds. */
+export const DEFAULT_TICK_S = 0.01;
+/** SimLoop's `HeadlessOpModeRunner.TICK_SECONDS`: a Gradle headless sim's 50 Hz loop. */
+export const GRADLE_TICK_S = 0.02;
+/**
+ * Seconds the sim runs past the auto period before it gives up on a routine that has not finished:
+ * the same margin a Gradle headless sim gives a 30 s period (1700 ticks of 0.02 s is 34 s).
+ */
+export const PERIOD_MARGIN_S = 4;
+/** When the field declares no auto period, the sim stops here. FTC's auto period is 30 s. */
+export const DEFAULT_PERIOD_S = 30;
+/** SimLoop's `FakeMotor.EPS`. */
+export const STOP_SNAP_FRACTION = 0.02;
+/** `MecanumConfig.powerThreshold`'s default in Pedro's revhub. */
+export const POWER_THRESHOLD = 0.01;
+/**
+ * Pedro's `ForesightConfig` defaults, for the knobs Pedro gives a default to. SPEC: copied from
+ * `com/pedropathing/algorithm/ForesightConfig.java` in core-3.0.0-20260828.185437-17.
+ */
+export const PEDRO_DEFAULTS = {
+    headingPowerPerRad: 1.5,
+    headingStaticPower: 0,
+    forwardTranslationalPowerPerIn: 0.07,
+    strafeTranslationalPowerPerIn: 0.1,
+    brakeFeedforwardPowerPerInPerS: 0.009,
+    coastPowerPerInPerS: 0.025,
+    coastFeedforwardPowerPerInPerS: 0.015,
+    maxBrakingPower: 0.2,
+    headingDriveRatio: 0.5,
+    cosineScale: true,
+    turnBeforeDriving: false,
+    brakeAggression: 1,
+    endParametricT: 0.025,
+    endHeadingToleranceRad: 0.007,
+    endTranslationalToleranceIn: 0.1,
+    endVelocityToleranceInPerS: 0.1,
+    holdTimeoutS: 0.1,
+    headingDeviationToleranceRad: (11.25 * Math.PI) / 180,
+    translationalDeviationToleranceIn: 2.5,
+    minCorrectionDistanceIn: 1e-3,
+};
+const isRecord = (value) => typeof value === "object" && value !== null && !Array.isArray(value);
+/** A `{ value, provenance }` number or a bare number under `source[key]`, when there is one. */
+function numberAt(source, key) {
+    if (!isRecord(source))
+        return undefined;
+    const raw = source[key];
+    if (typeof raw === "number" && Number.isFinite(raw))
+        return raw;
+    if (isRecord(raw) && typeof raw["value"] === "number" && Number.isFinite(raw["value"])) {
+        return raw["value"];
+    }
+    return undefined;
+}
+function booleanAt(source, key) {
+    if (!isRecord(source))
+        return undefined;
+    const raw = source[key];
+    if (typeof raw === "boolean")
+        return raw;
+    if (isRecord(raw) && typeof raw["value"] === "boolean")
+        return raw["value"];
+    return undefined;
+}
+/** `kinematics.follower` as a plain record, so keys the schema does not type yet can be read. */
+const followerSection = (robot) => robot.kinematics["follower"];
+/** `kinematics.drivetrain`, the plant section the handoff asks for. */
+const drivetrainSection = (robot) => robot.kinematics["drivetrain"];
+/**
+ * The follower the sim runs. Pedro's defaults except where a Pedro default would make the preview
+ * meaningless for any robot that has not been tuned, and there the value is derived:
+ *
+ * - `coastFeedforwardPowerPerInPerS` defaults to 1 / maxForwardVel, the power that holds a target
+ *   velocity on this robot's measured top speed (a robot with a 100 in/s top speed gets 0.01).
+ *   Pedro's 0.015 would push past full power at any target, so `speedFraction` would do nothing.
+ * - `coastPowerPerInPerS` defaults to 0 for the same reason: V3 feeds the coast controller the
+ *   target velocity as its "error", so a P term there is a second feedforward.
+ * - The brake coefficients, which Pedro makes `required()` and so has no default for, come from the
+ *   measured coasting deceleration: stopping distance v^2 / 2a, linear term 0. APPROX: the real
+ *   coefficients come from Pedro's braking identification OpModes and are shorter, because they
+ *   brake under reverse power rather than coast.
+ */
+export function followerParams(robot, options = {}) {
+    const k = robot.kinematics;
+    const section = followerSection(robot);
+    const forwardVel = k.maxForwardVelInPerS.value;
+    const leverArm = plantLeverArmIn(robot);
+    const read = (key, fallback) => options.follower?.[key] ?? numberAt(section, key) ?? fallback;
+    const flag = (key, fallback) => options.follower?.[key] ?? booleanAt(section, key) ?? fallback;
+    const forwardDecel = k.forwardDecelInPerS2.value;
+    const strafeDecel = k.strafeDecelInPerS2.value;
+    // The heading's natural deceleration is the forward one acting at the lever arm, the same
+    // geometry the plant turns with.
+    const headingDecel = forwardDecel / leverArm;
+    return {
+        forwardTranslationalPowerPerIn: read("forwardTranslationalPowerPerIn", PEDRO_DEFAULTS.forwardTranslationalPowerPerIn),
+        strafeTranslationalPowerPerIn: read("strafeTranslationalPowerPerIn", PEDRO_DEFAULTS.strafeTranslationalPowerPerIn),
+        headingPowerPerRad: read("headingPowerPerRad", PEDRO_DEFAULTS.headingPowerPerRad),
+        headingStaticPower: read("headingStaticPower", PEDRO_DEFAULTS.headingStaticPower),
+        coastPowerPerInPerS: read("coastPowerPerInPerS", 0),
+        coastFeedforwardPowerPerInPerS: read("coastFeedforwardPowerPerInPerS", 1 / forwardVel),
+        brakeFeedforwardPowerPerInPerS: read("brakeFeedforwardPowerPerInPerS", PEDRO_DEFAULTS.brakeFeedforwardPowerPerInPerS),
+        maxBrakingPower: read("maxBrakingPower", PEDRO_DEFAULTS.maxBrakingPower),
+        headingDriveRatio: read("headingDriveRatio", PEDRO_DEFAULTS.headingDriveRatio),
+        cosineScale: flag("cosineScale", PEDRO_DEFAULTS.cosineScale),
+        turnBeforeDriving: flag("turnBeforeDriving", PEDRO_DEFAULTS.turnBeforeDriving),
+        brakeAggression: read("brakeAggression", PEDRO_DEFAULTS.brakeAggression),
+        brakeLinearForwardS: read("brakeLinearForwardS", 0),
+        brakeQuadraticForwardS2PerIn: read("brakeQuadraticForwardS2PerIn", 1 / (2 * forwardDecel)),
+        brakeLinearStrafeS: read("brakeLinearStrafeS", 0),
+        brakeQuadraticStrafeS2PerIn: read("brakeQuadraticStrafeS2PerIn", 1 / (2 * strafeDecel)),
+        headingBrakeLinearS: read("headingBrakeLinearS", 0),
+        headingBrakeQuadraticS2PerRad: read("headingBrakeQuadraticS2PerRad", 1 / (2 * headingDecel)),
+        endParametricT: read("endParametricT", PEDRO_DEFAULTS.endParametricT),
+        endHeadingToleranceRad: read("endHeadingToleranceRad", PEDRO_DEFAULTS.endHeadingToleranceRad),
+        endTranslationalToleranceIn: read("endTranslationalToleranceIn", PEDRO_DEFAULTS.endTranslationalToleranceIn),
+        endVelocityToleranceInPerS: read("endVelocityToleranceInPerS", PEDRO_DEFAULTS.endVelocityToleranceInPerS),
+        holdTimeoutS: read("holdTimeoutS", PEDRO_DEFAULTS.holdTimeoutS),
+        headingDeviationToleranceRad: read("headingDeviationToleranceRad", PEDRO_DEFAULTS.headingDeviationToleranceRad),
+        translationalDeviationToleranceIn: read("translationalDeviationToleranceIn", PEDRO_DEFAULTS.translationalDeviationToleranceIn),
+        minCorrectionDistanceIn: read("minCorrectionDistanceIn", PEDRO_DEFAULTS.minCorrectionDistanceIn),
+    };
+}
+/**
+ * The mecanum lever arm: half the wheel base plus half the track width from
+ * `kinematics.drivetrain` when the file carries them, otherwise the arm that makes a full turn
+ * command reach `maxAngularVelRadPerS`, so the plant's angular limit is the one `robot.json` states.
+ */
+export function plantLeverArmIn(robot) {
+    const section = drivetrainSection(robot);
+    const trackWidth = numberAt(section, "trackWidthIn");
+    const wheelBase = numberAt(section, "wheelBaseIn");
+    if (trackWidth !== undefined && wheelBase !== undefined && trackWidth + wheelBase > 0) {
+        return wheelBase / 2 + trackWidth / 2;
+    }
+    return robot.kinematics.maxForwardVelInPerS.value / robot.kinematics.maxAngularVelRadPerS.value;
+}
+/**
+ * The plant. The wheel response rate is `kinematics.drivetrain.wheelResponseRatePerS` when present
+ * (a robot's own Gradle sim sets the same rate), otherwise 2 * accel / maxForwardVel:
+ * the rate at which a first-order wheel loses the same time to reaching top speed, V / (2a), as the
+ * constant-acceleration estimate does, so the two agree about what `accelInPerS2` means.
+ */
+export function plantParams(robot, field, fidelity, options = {}) {
+    const limits = robotLimits(robot);
+    const section = drivetrainSection(robot);
+    const defaults = {
+        maxForwardVelInPerS: limits.maxForwardVelInPerS,
+        maxStrafeVelInPerS: Math.min(limits.maxStrafeVelInPerS, limits.maxForwardVelInPerS),
+        wheelResponseRatePerS: numberAt(section, "wheelResponseRatePerS") ??
+            (2 * limits.accelInPerS2) / limits.maxForwardVelInPerS,
+        leverArmIn: plantLeverArmIn(robot),
+        forwardDecelInPerS2: limits.forwardDecelInPerS2,
+        strafeDecelInPerS2: limits.strafeDecelInPerS2,
+        coast: fidelity === "gradle" ? "firstOrder" : "natural",
+        stopSnapFraction: STOP_SNAP_FRACTION,
+        powerThreshold: POWER_THRESHOLD,
+        fieldHalfIn: Math.min(field.sizeIn.xIn, field.sizeIn.yIn) / 2,
+    };
+    return { ...defaults, ...definedOnly(options.plant) };
+}
+function definedOnly(partial) {
+    const out = {};
+    if (partial === undefined)
+        return out;
+    for (const [key, value] of Object.entries(partial)) {
+        if (value !== undefined)
+            out[key] = value;
+    }
+    return out;
+}
+/** The tick, the fidelity and the stopping time, resolved. */
+export function runParams(field, options) {
+    const fidelity = options.fidelity ?? "robot";
+    const tickS = options.tickS ?? (fidelity === "gradle" ? GRADLE_TICK_S : DEFAULT_TICK_S);
+    if (!(tickS > 0) || !Number.isFinite(tickS))
+        throw new RangeError("tickS must be a positive number of seconds.");
+    const periodS = field.periods?.autoS ?? null;
+    const maxTimeS = options.maxTimeS ?? (periodS ?? DEFAULT_PERIOD_S) + PERIOD_MARGIN_S;
+    return { tickS, fidelity, maxTimeS, periodS };
+}
+//# sourceMappingURL=params.js.map

package/dist/sim/params.js.map

@@ -0,0 +1 @@
+{"version":3,"file":"params.js","sourceRoot":"","sources":["../../src/sim/params.ts"],"names":[],"mappings":"AACA,OAAO,EAAE,WAAW,EAAE,MAAM,kBAAkB,CAAC;AAG/C;;;;;;;;;;;GAWG;AAEH,oDAAoD;AACpD,MAAM,CAAC,MAAM,cAAc,GAAG,IAAI,CAAC;AAEnC,yFAAyF;AACzF,MAAM,CAAC,MAAM,aAAa,GAAG,IAAI,CAAC;AAElC;;;GAGG;AACH,MAAM,CAAC,MAAM,eAAe,GAAG,CAAC,CAAC;AAEjC,6FAA6F;AAC7F,MAAM,CAAC,MAAM,gBAAgB,GAAG,EAAE,CAAC;AAEnC,iCAAiC;AACjC,MAAM,CAAC,MAAM,kBAAkB,GAAG,IAAI,CAAC;AAEvC,kEAAkE;AAClE,MAAM,CAAC,MAAM,eAAe,GAAG,IAAI,CAAC;AAEpC;;;GAGG;AACH,MAAM,CAAC,MAAM,cAAc,GAAG;IAC5B,kBAAkB,EAAE,GAAG;IACvB,kBAAkB,EAAE,CAAC;IACrB,8BAA8B,EAAE,IAAI;IACpC,6BAA6B,EAAE,GAAG;IAClC,8BAA8B,EAAE,KAAK;IACrC,mBAAmB,EAAE,KAAK;IAC1B,8BAA8B,EAAE,KAAK;IACrC,eAAe,EAAE,GAAG;IACpB,iBAAiB,EAAE,GAAG;IACtB,WAAW,EAAE,IAAI;IACjB,iBAAiB,EAAE,KAAK;IACxB,eAAe,EAAE,CAAC;IAClB,cAAc,EAAE,KAAK;IACrB,sBAAsB,EAAE,KAAK;IAC7B,2BAA2B,EAAE,GAAG;IAChC,0BAA0B,EAAE,GAAG;IAC/B,YAAY,EAAE,GAAG;IACjB,4BAA4B,EAAE,CAAC,KAAK,GAAG,IAAI,CAAC,EAAE,CAAC,GAAG,GAAG;IACrD,iCAAiC,EAAE,GAAG;IACtC,uBAAuB,EAAE,IAAI;CACrB,CAAC;AAEX,MAAM,QAAQ,GAAG,CAAC,KAAc,EAAoC,EAAE,CACpE,OAAO,KAAK,KAAK,QAAQ,IAAI,KAAK,KAAK,IAAI,IAAI,CAAC,KAAK,CAAC,OAAO,CAAC,KAAK,CAAC,CAAC;AAEvE,gGAAgG;AAChG,SAAS,QAAQ,CAAC,MAAe,EAAE,GAAW;IAC5C,IAAI,CAAC,QAAQ,CAAC,MAAM,CAAC;QAAE,OAAO,SAAS,CAAC;IACxC,MAAM,GAAG,GAAG,MAAM,CAAC,GAAG,CAAC,CAAC;IACxB,IAAI,OAAO,GAAG,KAAK,QAAQ,IAAI,MAAM,CAAC,QAAQ,CAAC,GAAG,CAAC;QAAE,OAAO,GAAG,CAAC;IAChE,IAAI,QAAQ,CAAC,GAAG,CAAC,IAAI,OAAO,GAAG,CAAC,OAAO,CAAC,KAAK,QAAQ,IAAI,MAAM,CAAC,QAAQ,CAAC,GAAG,CAAC,OAAO,CAAC,CAAC,EAAE,CAAC;QACvF,OAAO,GAAG,CAAC,OAAO,CAAC,CAAC;IACtB,CAAC;IACD,OAAO,SAAS,CAAC;AACnB,CAAC;AAED,SAAS,SAAS,CAAC,MAAe,EAAE,GAAW;IAC7C,IAAI,CAAC,QAAQ,CAAC,MAAM,CAAC;QAAE,OAAO,SAAS,CAAC;IACxC,MAAM,GAAG,GAAG,MAAM,CAAC,GAAG,CAAC,CAAC;IACxB,IAAI,OAAO,GAAG,KAAK,SAAS;QAAE,OAAO,GAAG,CAAC;IACzC,IAAI,QAAQ,CAAC,GAAG,CAAC,IAAI,OAAO,GAAG,CAAC,OAAO,CAAC,KAAK,SAAS;QAAE,OAAO,GAAG,CAAC,OAAO,CAAC,CAAC;IAC5E,OAAO,SAAS,CAAC;AACnB,CAAC;AAED,iGAAiG;AACjG,MAAM,eAAe,GAAG,CAAC,KAAY,EAAW,EAAE,CAAE,KAAK,CAAC,UAAiD,CAAC,UAAU,CAAC,CAAC;AAExH,uEAAuE;AACvE,MAAM,iBAAiB,GAAG,CAAC,KAAY,EAAW,EAAE,CACjD,KAAK,CAAC,UAAiD,CAAC,YAAY,CAAC,CAAC;AAEzE;;;;;;;;;;;;;GAaG;AACH,MAAM,UAAU,cAAc,CAAC,KAAY,EAAE,UAAsB,EAAE;IACnE,MAAM,CAAC,GAAG,KAAK,CAAC,UAAU,CAAC;IAC3B,MAAM,OAAO,GAAG,eAAe,CAAC,KAAK,CAAC,CAAC;IACvC,MAAM,UAAU,GAAG,CAAC,CAAC,mBAAmB,CAAC,KAAK,CAAC;IAC/C,MAAM,QAAQ,GAAG,eAAe,CAAC,KAAK,CAAC,CAAC;IACxC,MAAM,IAAI,GAAG,CAAC,GAAyB,EAAE,QAAgB,EAAU,EAAE,CAClE,OAAO,CAAC,QAAQ,EAAE,CAAC,GAAG,CAAwB,IAAI,QAAQ,CAAC,OAAO,EAAE,GAAG,CAAC,IAAI,QAAQ,CAAC;IACxF,MAAM,IAAI,GAAG,CAAC,GAAwC,EAAE,QAAiB,EAAW,EAAE,CACpF,OAAO,CAAC,QAAQ,EAAE,CAAC,GAAG,CAAC,IAAI,SAAS,CAAC,OAAO,EAAE,GAAG,CAAC,IAAI,QAAQ,CAAC;IAEjE,MAAM,YAAY,GAAG,CAAC,CAAC,mBAAmB,CAAC,KAAK,CAAC;IACjD,MAAM,WAAW,GAAG,CAAC,CAAC,kBAAkB,CAAC,KAAK,CAAC;IAC/C,0FAA0F;IAC1F,iCAAiC;IACjC,MAAM,YAAY,GAAG,YAAY,GAAG,QAAQ,CAAC;IAE7C,OAAO;QACL,8BAA8B,EAAE,IAAI,CAClC,gCAAgC,EAChC,cAAc,CAAC,8BAA8B,CAC9C;QACD,6BAA6B,EAAE,IAAI,CACjC,+BAA+B,EAC/B,cAAc,CAAC,6BAA6B,CAC7C;QACD,kBAAkB,EAAE,IAAI,CAAC,oBAAoB,EAAE,cAAc,CAAC,kBAAkB,CAAC;QACjF,kBAAkB,EAAE,IAAI,CAAC,oBAAoB,EAAE,cAAc,CAAC,kBAAkB,CAAC;QACjF,mBAAmB,EAAE,IAAI,CAAC,qBAAqB,EAAE,CAAC,CAAC;QACnD,8BAA8B,EAAE,IAAI,CAAC,gCAAgC,EAAE,CAAC,GAAG,UAAU,CAAC;QACtF,8BAA8B,EAAE,IAAI,CAClC,gCAAgC,EAChC,cAAc,CAAC,8BAA8B,CAC9C;QACD,eAAe,EAAE,IAAI,CAAC,iBAAiB,EAAE,cAAc,CAAC,eAAe,CAAC;QACxE,iBAAiB,EAAE,IAAI,CAAC,mBAAmB,EAAE,cAAc,CAAC,iBAAiB,CAAC;QAC9E,WAAW,EAAE,IAAI,CAAC,aAAa,EAAE,cAAc,CAAC,WAAW,CAAC;QAC5D,iBAAiB,EAAE,IAAI,CAAC,mBAAmB,EAAE,cAAc,CAAC,iBAAiB,CAAC;QAC9E,eAAe,EAAE,IAAI,CAAC,iBAAiB,EAAE,cAAc,CAAC,eAAe,CAAC;QACxE,mBAAmB,EAAE,IAAI,CAAC,qBAAqB,EAAE,CAAC,CAAC;QACnD,4BAA4B,EAAE,IAAI,CAAC,8BAA8B,EAAE,CAAC,GAAG,CAAC,CAAC,GAAG,YAAY,CAAC,CAAC;QAC1F,kBAAkB,EAAE,IAAI,CAAC,oBAAoB,EAAE,CAAC,CAAC;QACjD,2BAA2B,EAAE,IAAI,CAAC,6BAA6B,EAAE,CAAC,GAAG,CAAC,CAAC,GAAG,WAAW,CAAC,CAAC;QACvF,mBAAmB,EAAE,IAAI,CAAC,qBAAqB,EAAE,CAAC,CAAC;QACnD,6BAA6B,EAAE,IAAI,CAAC,+BAA+B,EAAE,CAAC,GAAG,CAAC,CAAC,GAAG,YAAY,CAAC,CAAC;QAC5F,cAAc,EAAE,IAAI,CAAC,gBAAgB,EAAE,cAAc,CAAC,cAAc,CAAC;QACrE,sBAAsB,EAAE,IAAI,CAAC,wBAAwB,EAAE,cAAc,CAAC,sBAAsB,CAAC;QAC7F,2BAA2B,EAAE,IAAI,CAC/B,6BAA6B,EAC7B,cAAc,CAAC,2BAA2B,CAC3C;QACD,0BAA0B,EAAE,IAAI,CAC9B,4BAA4B,EAC5B,cAAc,CAAC,0BAA0B,CAC1C;QACD,YAAY,EAAE,IAAI,CAAC,cAAc,EAAE,cAAc,CAAC,YAAY,CAAC;QAC/D,4BAA4B,EAAE,IAAI,CAChC,8BAA8B,EAC9B,cAAc,CAAC,4BAA4B,CAC5C;QACD,iCAAiC,EAAE,IAAI,CACrC,mCAAmC,EACnC,cAAc,CAAC,iCAAiC,CACjD;QACD,uBAAuB,EAAE,IAAI,CAAC,yBAAyB,EAAE,cAAc,CAAC,uBAAuB,CAAC;KACjG,CAAC;AACJ,CAAC;AAED;;;;GAIG;AACH,MAAM,UAAU,eAAe,CAAC,KAAY;IAC1C,MAAM,OAAO,GAAG,iBAAiB,CAAC,KAAK,CAAC,CAAC;IACzC,MAAM,UAAU,GAAG,QAAQ,CAAC,OAAO,EAAE,cAAc,CAAC,CAAC;IACrD,MAAM,SAAS,GAAG,QAAQ,CAAC,OAAO,EAAE,aAAa,CAAC,CAAC;IACnD,IAAI,UAAU,KAAK,SAAS,IAAI,SAAS,KAAK,SAAS,IAAI,UAAU,GAAG,SAAS,GAAG,CAAC,EAAE,CAAC;QACtF,OAAO,SAAS,GAAG,CAAC,GAAG,UAAU,GAAG,CAAC,CAAC;IACxC,CAAC;IACD,OAAO,KAAK,CAAC,UAAU,CAAC,mBAAmB,CAAC,KAAK,GAAG,KAAK,CAAC,UAAU,CAAC,oBAAoB,CAAC,KAAK,CAAC;AAClG,CAAC;AAED;;;;;GAKG;AACH,MAAM,UAAU,WAAW,CACzB,KAAY,EACZ,KAAY,EACZ,QAAqB,EACrB,UAAsB,EAAE;IAExB,MAAM,MAAM,GAAG,WAAW,CAAC,KAAK,CAAC,CAAC;IAClC,MAAM,OAAO,GAAG,iBAAiB,CAAC,KAAK,CAAC,CAAC;IACzC,MAAM,QAAQ,GAAgB;QAC5B,mBAAmB,EAAE,MAAM,CAAC,mBAAmB;QAC/C,kBAAkB,EAAE,IAAI,CAAC,GAAG,CAAC,MAAM,CAAC,kBAAkB,EAAE,MAAM,CAAC,mBAAmB,CAAC;QACnF,qBAAqB,EACnB,QAAQ,CAAC,OAAO,EAAE,uBAAuB,CAAC;YAC1C,CAAC,CAAC,GAAG,MAAM,CAAC,YAAY,CAAC,GAAG,MAAM,CAAC,mBAAmB;QACxD,UAAU,EAAE,eAAe,CAAC,KAAK,CAAC;QAClC,mBAAmB,EAAE,MAAM,CAAC,mBAAmB;QAC/C,kBAAkB,EAAE,MAAM,CAAC,kBAAkB;QAC7C,KAAK,EAAE,QAAQ,KAAK,QAAQ,CAAC,CAAC,CAAC,YAAY,CAAC,CAAC,CAAC,SAAS;QACvD,gBAAgB,EAAE,kBAAkB;QACpC,cAAc,EAAE,eAAe;QAC/B,WAAW,EAAE,IAAI,CAAC,GAAG,CAAC,KAAK,CAAC,MAAM,CAAC,GAAG,EAAE,KAAK,CAAC,MAAM,CAAC,GAAG,CAAC,GAAG,CAAC;KAC9D,CAAC;IACF,OAAO,EAAE,GAAG,QAAQ,EAAE,GAAG,WAAW,CAAC,OAAO,CAAC,KAAK,CAAC,EAAE,CAAC;AACxD,CAAC;AAED,SAAS,WAAW,CAAmB,OAA+B;IACpE,MAAM,GAAG,GAAe,EAAE,CAAC;IAC3B,IAAI,OAAO,KAAK,SAAS;QAAE,OAAO,GAAG,CAAC;IACtC,KAAK,MAAM,CAAC,GAAG,EAAE,KAAK,CAAC,IAAI,MAAM,CAAC,OAAO,CAAC,OAAO,CAAC,EAAE,CAAC;QACnD,IAAI,KAAK,KAAK,SAAS;YAAG,GAA+B,CAAC,GAAG,CAAC,GAAG,KAAK,CAAC;IACzE,CAAC;IACD,OAAO,GAAG,CAAC;AACb,CAAC;AAED,8DAA8D;AAC9D,MAAM,UAAU,SAAS,CACvB,KAAY,EACZ,OAAmB;IAEnB,MAAM,QAAQ,GAAG,OAAO,CAAC,QAAQ,IAAI,OAAO,CAAC;IAC7C,MAAM,KAAK,GAAG,OAAO,CAAC,KAAK,IAAI,CAAC,QAAQ,KAAK,QAAQ,CAAC,CAAC,CAAC,aAAa,CAAC,CAAC,CAAC,cAAc,CAAC,CAAC;IACxF,IAAI,CAAC,CAAC,KAAK,GAAG,CAAC,CAAC,IAAI,CAAC,MAAM,CAAC,QAAQ,CAAC,KAAK,CAAC;QAAE,MAAM,IAAI,UAAU,CAAC,6CAA6C,CAAC,CAAC;IACjH,MAAM,OAAO,GAAG,KAAK,CAAC,OAAO,EAAE,KAAK,IAAI,IAAI,CAAC;IAC7C,MAAM,QAAQ,GAAG,OAAO,CAAC,QAAQ,IAAI,CAAC,OAAO,IAAI,gBAAgB,CAAC,GAAG,eAAe,CAAC;IACrF,OAAO,EAAE,KAAK,EAAE,QAAQ,EAAE,QAAQ,EAAE,OAAO,EAAE,CAAC;AAChD,CAAC"}

package/dist/sim/pedroPath.d.ts

@@ -0,0 +1,41 @@
+import type { Heading } from "@horizon36596/zenith-schema";
+import { type SimCurve, type XY } from "./curves.js";
+/** `Angle.normalize`: into [0, 2pi). */
+export declare function normalizeAngle(rad: number): number;
+/** `Angle.normalizeSigned`: into [-pi, pi). */
+export declare function normalizeSigned(rad: number): number;
+/** `Angle.turnDirection(normalizedError)`: +1 counter-clockwise, -1 clockwise. */
+export declare function turnDirection(error: number): number;
+/** `Vector2D.theta`: 0 for a zero vector. */
+export declare const theta: (v: XY) => number;
+export interface SimSegment {
+    readonly curve: SimCurve;
+    /** The target heading at this segment's parametric t, in [0, 2pi) like Pedro's. */
+    heading(t: number): number;
+}
+export interface SimPath {
+    readonly segments: readonly SimSegment[];
+    /** `PathBuilder.Built.segmentLengthsIn`: what the markers measure distance with. */
+    readonly runtimeLengthsIn: readonly number[];
+    readonly runtimeTotalIn: number;
+    /** `PathTracker.endPose`: the last segment's end with its heading at t = 1. */
+    readonly endPose: {
+        x: number;
+        y: number;
+        heading: number;
+    };
+    /** Segments dropped because they had no length, which Pedro's `Line` refuses. */
+    readonly droppedSegments: number;
+}
+/**
+ * The segment t at which the runtime's chord table reaches `fraction` of the segment's length:
+ * `PathBuilder.parameterAt`, 64 chords, linear inside a chord. Exact for a line.
+ */
+export declare function parameterAtFraction(curve: SimCurve, fraction: number): number;
+/**
+ * Builds the followable path from each segment's control polygon (start, controls, end), in the
+ * alliance frame the plan is in. `fallbackHeadingRad` is held when the step has no heading mode,
+ * which `validate` refuses but the sim still has to drive.
+ */
+export declare function buildSimPath(polygons: readonly (readonly XY[])[], heading: Heading | undefined, fallbackHeadingRad: number): SimPath;
+//# sourceMappingURL=pedroPath.d.ts.map

package/dist/sim/pedroPath.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"pedroPath.d.ts","sourceRoot":"","sources":["../../src/sim/pedroPath.ts"],"names":[],"mappings":"AAAA,OAAO,KAAK,EAAE,OAAO,EAA8B,MAAM,6BAA6B,CAAC;AACvF,OAAO,EAAwC,KAAK,QAAQ,EAAE,KAAK,EAAE,EAAE,MAAM,aAAa,CAAC;AAW3F,wCAAwC;AACxC,wBAAgB,cAAc,CAAC,GAAG,EAAE,MAAM,GAAG,MAAM,CAGlD;AAED,+CAA+C;AAC/C,wBAAgB,eAAe,CAAC,GAAG,EAAE,MAAM,GAAG,MAAM,CAGnD;AAED,kFAAkF;AAClF,wBAAgB,aAAa,CAAC,KAAK,EAAE,MAAM,GAAG,MAAM,CAEnD;AAED,6CAA6C;AAC7C,eAAO,MAAM,KAAK,GAAI,GAAG,EAAE,KAAG,MAAmE,CAAC;AAElG,MAAM,WAAW,UAAU;IACzB,QAAQ,CAAC,KAAK,EAAE,QAAQ,CAAC;IACzB,mFAAmF;IACnF,OAAO,CAAC,CAAC,EAAE,MAAM,GAAG,MAAM,CAAC;CAC5B;AAED,MAAM,WAAW,OAAO;IACtB,QAAQ,CAAC,QAAQ,EAAE,SAAS,UAAU,EAAE,CAAC;IACzC,oFAAoF;IACpF,QAAQ,CAAC,gBAAgB,EAAE,SAAS,MAAM,EAAE,CAAC;IAC7C,QAAQ,CAAC,cAAc,EAAE,MAAM,CAAC;IAChC,+EAA+E;IAC/E,QAAQ,CAAC,OAAO,EAAE;QAAE,CAAC,EAAE,MAAM,CAAC;QAAC,CAAC,EAAE,MAAM,CAAC;QAAC,OAAO,EAAE,MAAM,CAAA;KAAE,CAAC;IAC5D,iFAAiF;IACjF,QAAQ,CAAC,eAAe,EAAE,MAAM,CAAC;CAClC;AAsDD;;;GAGG;AACH,wBAAgB,mBAAmB,CAAC,KAAK,EAAE,QAAQ,EAAE,QAAQ,EAAE,MAAM,GAAG,MAAM,CAsB7E;AAgED;;;;GAIG;AACH,wBAAgB,YAAY,CAC1B,QAAQ,EAAE,SAAS,CAAC,SAAS,EAAE,EAAE,CAAC,EAAE,EACpC,OAAO,EAAE,OAAO,GAAG,SAAS,EAC5B,kBAAkB,EAAE,MAAM,GACzB,OAAO,CAqCT"}

package/dist/sim/pedroPath.js

@@ -0,0 +1,191 @@
+import { makeSimCurve, RUNTIME_LENGTH_SAMPLES } from "./curves.js";
+/**
+ * A path step as the robot runtime hands it to Pedro: `PathBuilder.build` and `withHeading` in
+ * `robot/auto-runtime` (the copy the biobuzz repository runs), then Pedro's own interpolators from
+ * `paths/interpolator/Interpolator.java`. The follower advances segment by segment, the way
+ * Pedro's `PathTracker` does, and each segment evaluates its heading at its own parametric t.
+ */
+const TWO_PI = Math.PI * 2;
+/** `Angle.normalize`: into [0, 2pi). */
+export function normalizeAngle(rad) {
+    const angle = rad % TWO_PI;
+    return angle < 0 ? angle + TWO_PI : angle;
+}
+/** `Angle.normalizeSigned`: into [-pi, pi). */
+export function normalizeSigned(rad) {
+    const angle = normalizeAngle(rad);
+    return angle >= Math.PI ? angle - TWO_PI : angle;
+}
+/** `Angle.turnDirection(normalizedError)`: +1 counter-clockwise, -1 clockwise. */
+export function turnDirection(error) {
+    return error >= 0 && error <= Math.PI ? 1 : -1;
+}
+/** `Vector2D.theta`: 0 for a zero vector. */
+export const theta = (v) => (v.x * v.x + v.y * v.y < 1e-9 ? 0 : Math.atan2(v.y, v.x));
+/**
+ * One segment's heading, exactly as `PathBuilder.withHeading` attaches it and Pedro evaluates it.
+ *
+ * Two places where that differs from `packages/core/src/heading.ts`, both recorded in the ADR:
+ *
+ * - `linear`: the runtime splits the authored sweep by arc length and hands each piece to
+ *   `Path.linear(from, to)`, but Pedro's `Interpolator.linear` takes the short way from `from` to
+ *   `to` (`Angle.error`), so a piece over half a turn reverses. It is also linear in the segment's
+ *   parametric t, not in arc length.
+ * - `facePoint`: the runtime passes only the point, so `offsetRad` is dropped.
+ * - `piecewise`: see `piecewiseSegmentHeading`.
+ */
+function segmentHeading(heading, curve, startFraction, endFraction, fallbackRad) {
+    if (heading === undefined) {
+        const fixed = normalizeAngle(fallbackRad);
+        return () => fixed;
+    }
+    switch (heading.mode) {
+        case "tangent":
+            return (t) => normalizeAngle(theta(curve.tangent(t)));
+        case "tangentReversed":
+            return (t) => normalizeAngle(theta(curve.tangent(t)) + Math.PI);
+        case "constant": {
+            const fixed = normalizeAngle(heading.headingRad);
+            return () => fixed;
+        }
+        case "linear": {
+            const sweep = heading.toRad - heading.fromRad;
+            const from = normalizeAngle(heading.fromRad + sweep * startFraction);
+            const to = normalizeAngle(heading.fromRad + sweep * endFraction);
+            const delta = normalizeSigned(to - from);
+            return (t) => normalizeAngle(from + delta * t);
+        }
+        case "facePoint": {
+            const px = heading.xIn;
+            const py = heading.yIn;
+            return (t) => {
+                const at = curve.get(t);
+                return normalizeAngle(theta({ x: px - at.x, y: py - at.y }));
+            };
+        }
+        case "piecewise":
+            return piecewiseSegmentHeading(heading.ranges, curve, startFraction, endFraction, fallbackRad);
+    }
+}
+/**
+ * The segment t at which the runtime's chord table reaches `fraction` of the segment's length:
+ * `PathBuilder.parameterAt`, 64 chords, linear inside a chord. Exact for a line.
+ */
+export function parameterAtFraction(curve, fraction) {
+    if (fraction <= 0)
+        return 0;
+    if (fraction >= 1)
+        return 1;
+    const cumulative = [0];
+    let previous = curve.get(0);
+    for (let index = 1; index <= RUNTIME_LENGTH_SAMPLES; index += 1) {
+        const point = curve.get(index / RUNTIME_LENGTH_SAMPLES);
+        cumulative.push(cumulative[index - 1] + Math.hypot(point.x - previous.x, point.y - previous.y));
+        previous = point;
+    }
+    const total = cumulative[RUNTIME_LENGTH_SAMPLES];
+    if (total <= 0)
+        return fraction;
+    const target = fraction * total;
+    for (let index = 1; index <= RUNTIME_LENGTH_SAMPLES; index += 1) {
+        const upper = cumulative[index];
+        if (upper >= target) {
+            const lower = cumulative[index - 1];
+            const within = upper === lower ? 0 : (target - lower) / (upper - lower);
+            return (index - 1 + within) / RUNTIME_LENGTH_SAMPLES;
+        }
+    }
+    return 1;
+}
+/**
+ * A `piecewise` heading on one segment, the way `PathBuilder.piecewiseFor` builds it: the ranges
+ * that cross this segment are clipped to it, their ends are turned from the step's arc-length
+ * fractions into this segment's own t through the chord table, and Pedro's `PiecewiseInterpolator`
+ * picks the first range whose end is at or past t. A `linear` range turns the short way between its
+ * headings at the two clipped ends, linear in the segment's t; every other mode is Pedro's own.
+ */
+function piecewiseSegmentHeading(ranges, curve, startFraction, endFraction, fallbackRad) {
+    const span = endFraction - startFraction;
+    const pieces = [];
+    let previousT = 0;
+    for (const range of ranges) {
+        const from = Math.max(range.startT, startFraction);
+        const to = Math.min(range.endT, endFraction);
+        if (to <= from && !(span <= 0 && range === ranges[ranges.length - 1]))
+            continue;
+        const localFrom = span <= 0 ? 0 : parameterAtFraction(curve, (from - startFraction) / span);
+        const localTo = span <= 0 ? 1 : parameterAtFraction(curve, (to - startFraction) / span);
+        if (localTo <= previousT)
+            continue;
+        pieces.push({ endT: localTo, evaluate: rangeSegmentHeading(range, curve, from, to, localFrom, localTo, fallbackRad) });
+        previousT = localTo;
+    }
+    if (pieces.length === 0) {
+        const fixed = normalizeAngle(fallbackRad);
+        return () => fixed;
+    }
+    pieces[pieces.length - 1].endT = 1;
+    return (t) => {
+        const piece = pieces.find((candidate) => t <= candidate.endT) ?? pieces[pieces.length - 1];
+        return piece.evaluate(t);
+    };
+}
+function rangeSegmentHeading(range, curve, from, to, localFrom, localTo, fallbackRad) {
+    const inner = range.heading;
+    if (inner.mode !== "linear")
+        return segmentHeading(inner, curve, 0, 1, fallbackRad);
+    const rangeSpan = range.endT - range.startT;
+    const turn = normalizeSigned(inner.toRad - inner.fromRad);
+    const at = (fraction) => inner.fromRad + turn * (rangeSpan <= 0 ? 1 : (fraction - range.startT) / rangeSpan);
+    const headFrom = normalizeAngle(at(from));
+    const delta = at(to) - at(from);
+    const localSpan = localTo - localFrom;
+    return (t) => {
+        const local = localSpan <= 0 ? 1 : Math.min(1, Math.max(0, (t - localFrom) / localSpan));
+        return normalizeAngle(headFrom + delta * local);
+    };
+}
+/**
+ * Builds the followable path from each segment's control polygon (start, controls, end), in the
+ * alliance frame the plan is in. `fallbackHeadingRad` is held when the step has no heading mode,
+ * which `validate` refuses but the sim still has to drive.
+ */
+export function buildSimPath(polygons, heading, fallbackHeadingRad) {
+    const kept = polygons.filter((points) => {
+        const first = points[0];
+        const last = points[points.length - 1];
+        if (first === undefined || last === undefined)
+            return false;
+        if (points.length > 2)
+            return true;
+        return Math.hypot(last.x - first.x, last.y - first.y) > 0;
+    });
+    const curves = kept.map((points) => makeSimCurve(points));
+    const lengths = curves.map((curve) => curve.runtimeLengthIn);
+    const total = lengths.reduce((sum, value) => sum + value, 0);
+    let travelled = 0;
+    const segments = curves.map((curve, index) => {
+        const startFraction = total === 0 ? 0 : travelled / total;
+        travelled += lengths[index];
+        const endFraction = total === 0 ? 1 : travelled / total;
+        return {
+            curve,
+            heading: segmentHeading(heading, curve, startFraction, endFraction, fallbackHeadingRad),
+        };
+    });
+    const last = segments[segments.length - 1];
+    const lastPolygon = polygons[polygons.length - 1];
+    const endPoint = last?.curve.end ?? lastPolygon?.[lastPolygon.length - 1] ?? { x: 0, y: 0 };
+    return {
+        segments,
+        runtimeLengthsIn: lengths,
+        runtimeTotalIn: total,
+        endPose: {
+            x: endPoint.x,
+            y: endPoint.y,
+            heading: last === undefined ? normalizeAngle(fallbackHeadingRad) : last.heading(1),
+        },
+        droppedSegments: polygons.length - kept.length,
+    };
+}
+//# sourceMappingURL=pedroPath.js.map

package/dist/sim/pedroPath.js.map

@@ -0,0 +1 @@
+{"version":3,"file":"pedroPath.js","sourceRoot":"","sources":["../../src/sim/pedroPath.ts"],"names":[],"mappings":"AACA,OAAO,EAAE,YAAY,EAAE,sBAAsB,EAA0B,MAAM,aAAa,CAAC;AAE3F;;;;;GAKG;AAEH,MAAM,MAAM,GAAG,IAAI,CAAC,EAAE,GAAG,CAAC,CAAC;AAE3B,wCAAwC;AACxC,MAAM,UAAU,cAAc,CAAC,GAAW;IACxC,MAAM,KAAK,GAAG,GAAG,GAAG,MAAM,CAAC;IAC3B,OAAO,KAAK,GAAG,CAAC,CAAC,CAAC,CAAC,KAAK,GAAG,MAAM,CAAC,CAAC,CAAC,KAAK,CAAC;AAC5C,CAAC;AAED,+CAA+C;AAC/C,MAAM,UAAU,eAAe,CAAC,GAAW;IACzC,MAAM,KAAK,GAAG,cAAc,CAAC,GAAG,CAAC,CAAC;IAClC,OAAO,KAAK,IAAI,IAAI,CAAC,EAAE,CAAC,CAAC,CAAC,KAAK,GAAG,MAAM,CAAC,CAAC,CAAC,KAAK,CAAC;AACnD,CAAC;AAED,kFAAkF;AAClF,MAAM,UAAU,aAAa,CAAC,KAAa;IACzC,OAAO,KAAK,IAAI,CAAC,IAAI,KAAK,IAAI,IAAI,CAAC,EAAE,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC;AACjD,CAAC;AAED,6CAA6C;AAC7C,MAAM,CAAC,MAAM,KAAK,GAAG,CAAC,CAAK,EAAU,EAAE,CAAC,CAAC,CAAC,CAAC,CAAC,GAAG,CAAC,CAAC,CAAC,GAAG,CAAC,CAAC,CAAC,GAAG,CAAC,CAAC,CAAC,GAAG,IAAI,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,IAAI,CAAC,KAAK,CAAC,CAAC,CAAC,CAAC,EAAE,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC;AAmBlG;;;;;;;;;;;GAWG;AACH,SAAS,cAAc,CACrB,OAA4B,EAC5B,KAAe,EACf,aAAqB,EACrB,WAAmB,EACnB,WAAmB;IAEnB,IAAI,OAAO,KAAK,SAAS,EAAE,CAAC;QAC1B,MAAM,KAAK,GAAG,cAAc,CAAC,WAAW,CAAC,CAAC;QAC1C,OAAO,GAAG,EAAE,CAAC,KAAK,CAAC;IACrB,CAAC;IACD,QAAQ,OAAO,CAAC,IAAI,EAAE,CAAC;QACrB,KAAK,SAAS;YACZ,OAAO,CAAC,CAAC,EAAE,EAAE,CAAC,cAAc,CAAC,KAAK,CAAC,KAAK,CAAC,OAAO,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC;QACxD,KAAK,iBAAiB;YACpB,OAAO,CAAC,CAAC,EAAE,EAAE,CAAC,cAAc,CAAC,KAAK,CAAC,KAAK,CAAC,OAAO,CAAC,CAAC,CAAC,CAAC,GAAG,IAAI,CAAC,EAAE,CAAC,CAAC;QAClE,KAAK,UAAU,CAAC,CAAC,CAAC;YAChB,MAAM,KAAK,GAAG,cAAc,CAAC,OAAO,CAAC,UAAU,CAAC,CAAC;YACjD,OAAO,GAAG,EAAE,CAAC,KAAK,CAAC;QACrB,CAAC;QACD,KAAK,QAAQ,CAAC,CAAC,CAAC;YACd,MAAM,KAAK,GAAG,OAAO,CAAC,KAAK,GAAG,OAAO,CAAC,OAAO,CAAC;YAC9C,MAAM,IAAI,GAAG,cAAc,CAAC,OAAO,CAAC,OAAO,GAAG,KAAK,GAAG,aAAa,CAAC,CAAC;YACrE,MAAM,EAAE,GAAG,cAAc,CAAC,OAAO,CAAC,OAAO,GAAG,KAAK,GAAG,WAAW,CAAC,CAAC;YACjE,MAAM,KAAK,GAAG,eAAe,CAAC,EAAE,GAAG,IAAI,CAAC,CAAC;YACzC,OAAO,CAAC,CAAC,EAAE,EAAE,CAAC,cAAc,CAAC,IAAI,GAAG,KAAK,GAAG,CAAC,CAAC,CAAC;QACjD,CAAC;QACD,KAAK,WAAW,CAAC,CAAC,CAAC;YACjB,MAAM,EAAE,GAAG,OAAO,CAAC,GAAG,CAAC;YACvB,MAAM,EAAE,GAAG,OAAO,CAAC,GAAG,CAAC;YACvB,OAAO,CAAC,CAAC,EAAE,EAAE;gBACX,MAAM,EAAE,GAAG,KAAK,CAAC,GAAG,CAAC,CAAC,CAAC,CAAC;gBACxB,OAAO,cAAc,CAAC,KAAK,CAAC,EAAE,CAAC,EAAE,EAAE,GAAG,EAAE,CAAC,CAAC,EAAE,CAAC,EAAE,EAAE,GAAG,EAAE,CAAC,CAAC,EAAE,CAAC,CAAC,CAAC;YAC/D,CAAC,CAAC;QACJ,CAAC;QACD,KAAK,WAAW;YACd,OAAO,uBAAuB,CAAC,OAAO,CAAC,MAAM,EAAE,KAAK,EAAE,aAAa,EAAE,WAAW,EAAE,WAAW,CAAC,CAAC;IACnG,CAAC;AACH,CAAC;AAED;;;GAGG;AACH,MAAM,UAAU,mBAAmB,CAAC,KAAe,EAAE,QAAgB;IACnE,IAAI,QAAQ,IAAI,CAAC;QAAE,OAAO,CAAC,CAAC;IAC5B,IAAI,QAAQ,IAAI,CAAC;QAAE,OAAO,CAAC,CAAC;IAC5B,MAAM,UAAU,GAAa,CAAC,CAAC,CAAC,CAAC;IACjC,IAAI,QAAQ,GAAG,KAAK,CAAC,GAAG,CAAC,CAAC,CAAC,CAAC;IAC5B,KAAK,IAAI,KAAK,GAAG,CAAC,EAAE,KAAK,IAAI,sBAAsB,EAAE,KAAK,IAAI,CAAC,EAAE,CAAC;QAChE,MAAM,KAAK,GAAG,KAAK,CAAC,GAAG,CAAC,KAAK,GAAG,sBAAsB,CAAC,CAAC;QACxD,UAAU,CAAC,IAAI,CAAE,UAAU,CAAC,KAAK,GAAG,CAAC,CAAY,GAAG,IAAI,CAAC,KAAK,CAAC,KAAK,CAAC,CAAC,GAAG,QAAQ,CAAC,CAAC,EAAE,KAAK,CAAC,CAAC,GAAG,QAAQ,CAAC,CAAC,CAAC,CAAC,CAAC;QAC5G,QAAQ,GAAG,KAAK,CAAC;IACnB,CAAC;IACD,MAAM,KAAK,GAAG,UAAU,CAAC,sBAAsB,CAAW,CAAC;IAC3D,IAAI,KAAK,IAAI,CAAC;QAAE,OAAO,QAAQ,CAAC;IAChC,MAAM,MAAM,GAAG,QAAQ,GAAG,KAAK,CAAC;IAChC,KAAK,IAAI,KAAK,GAAG,CAAC,EAAE,KAAK,IAAI,sBAAsB,EAAE,KAAK,IAAI,CAAC,EAAE,CAAC;QAChE,MAAM,KAAK,GAAG,UAAU,CAAC,KAAK,CAAW,CAAC;QAC1C,IAAI,KAAK,IAAI,MAAM,EAAE,CAAC;YACpB,MAAM,KAAK,GAAG,UAAU,CAAC,KAAK,GAAG,CAAC,CAAW,CAAC;YAC9C,MAAM,MAAM,GAAG,KAAK,KAAK,KAAK,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,MAAM,GAAG,KAAK,CAAC,GAAG,CAAC,KAAK,GAAG,KAAK,CAAC,CAAC;YACxE,OAAO,CAAC,KAAK,GAAG,CAAC,GAAG,MAAM,CAAC,GAAG,sBAAsB,CAAC;QACvD,CAAC;IACH,CAAC;IACD,OAAO,CAAC,CAAC;AACX,CAAC;AAED;;;;;;GAMG;AACH,SAAS,uBAAuB,CAC9B,MAA+B,EAC/B,KAAe,EACf,aAAqB,EACrB,WAAmB,EACnB,WAAmB;IAEnB,MAAM,IAAI,GAAG,WAAW,GAAG,aAAa,CAAC;IACzC,MAAM,MAAM,GAAwD,EAAE,CAAC;IACvE,IAAI,SAAS,GAAG,CAAC,CAAC;IAClB,KAAK,MAAM,KAAK,IAAI,MAAM,EAAE,CAAC;QAC3B,MAAM,IAAI,GAAG,IAAI,CAAC,GAAG,CAAC,KAAK,CAAC,MAAM,EAAE,aAAa,CAAC,CAAC;QACnD,MAAM,EAAE,GAAG,IAAI,CAAC,GAAG,CAAC,KAAK,CAAC,IAAI,EAAE,WAAW,CAAC,CAAC;QAC7C,IAAI,EAAE,IAAI,IAAI,IAAI,CAAC,CAAC,IAAI,IAAI,CAAC,IAAI,KAAK,KAAK,MAAM,CAAC,MAAM,CAAC,MAAM,GAAG,CAAC,CAAC,CAAC;YAAE,SAAS;QAChF,MAAM,SAAS,GAAG,IAAI,IAAI,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,mBAAmB,CAAC,KAAK,EAAE,CAAC,IAAI,GAAG,aAAa,CAAC,GAAG,IAAI,CAAC,CAAC;QAC5F,MAAM,OAAO,GAAG,IAAI,IAAI,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,mBAAmB,CAAC,KAAK,EAAE,CAAC,EAAE,GAAG,aAAa,CAAC,GAAG,IAAI,CAAC,CAAC;QACxF,IAAI,OAAO,IAAI,SAAS;YAAE,SAAS;QACnC,MAAM,CAAC,IAAI,CAAC,EAAE,IAAI,EAAE,OAAO,EAAE,QAAQ,EAAE,mBAAmB,CAAC,KAAK,EAAE,KAAK,EAAE,IAAI,EAAE,EAAE,EAAE,SAAS,EAAE,OAAO,EAAE,WAAW,CAAC,EAAE,CAAC,CAAC;QACvH,SAAS,GAAG,OAAO,CAAC;IACtB,CAAC;IACD,IAAI,MAAM,CAAC,MAAM,KAAK,CAAC,EAAE,CAAC;QACxB,MAAM,KAAK,GAAG,cAAc,CAAC,WAAW,CAAC,CAAC;QAC1C,OAAO,GAAG,EAAE,CAAC,KAAK,CAAC;IACrB,CAAC;IACA,MAAM,CAAC,MAAM,CAAC,MAAM,GAAG,CAAC,CAAsB,CAAC,IAAI,GAAG,CAAC,CAAC;IACzD,OAAO,CAAC,CAAC,EAAE,EAAE;QACX,MAAM,KAAK,GAAG,MAAM,CAAC,IAAI,CAAC,CAAC,SAAS,EAAE,EAAE,CAAC,CAAC,IAAI,SAAS,CAAC,IAAI,CAAC,IAAK,MAAM,CAAC,MAAM,CAAC,MAAM,GAAG,CAAC,CAA6B,CAAC;QACxH,OAAO,KAAK,CAAC,QAAQ,CAAC,CAAC,CAAC,CAAC;IAC3B,CAAC,CAAC;AACJ,CAAC;AAED,SAAS,mBAAmB,CAC1B,KAAmB,EACnB,KAAe,EACf,IAAY,EACZ,EAAU,EACV,SAAiB,EACjB,OAAe,EACf,WAAmB;IAEnB,MAAM,KAAK,GAAiB,KAAK,CAAC,OAAO,CAAC;IAC1C,IAAI,KAAK,CAAC,IAAI,KAAK,QAAQ;QAAE,OAAO,cAAc,CAAC,KAAK,EAAE,KAAK,EAAE,CAAC,EAAE,CAAC,EAAE,WAAW,CAAC,CAAC;IACpF,MAAM,SAAS,GAAG,KAAK,CAAC,IAAI,GAAG,KAAK,CAAC,MAAM,CAAC;IAC5C,MAAM,IAAI,GAAG,eAAe,CAAC,KAAK,CAAC,KAAK,GAAG,KAAK,CAAC,OAAO,CAAC,CAAC;IAC1D,MAAM,EAAE,GAAG,CAAC,QAAgB,EAAU,EAAE,CACtC,KAAK,CAAC,OAAO,GAAG,IAAI,GAAG,CAAC,SAAS,IAAI,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,QAAQ,GAAG,KAAK,CAAC,MAAM,CAAC,GAAG,SAAS,CAAC,CAAC;IACtF,MAAM,QAAQ,GAAG,cAAc,CAAC,EAAE,CAAC,IAAI,CAAC,CAAC,CAAC;IAC1C,MAAM,KAAK,GAAG,EAAE,CAAC,EAAE,CAAC,GAAG,EAAE,CAAC,IAAI,CAAC,CAAC;IAChC,MAAM,SAAS,GAAG,OAAO,GAAG,SAAS,CAAC;IACtC,OAAO,CAAC,CAAC,EAAE,EAAE;QACX,MAAM,KAAK,GAAG,SAAS,IAAI,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,IAAI,CAAC,GAAG,CAAC,CAAC,EAAE,IAAI,CAAC,GAAG,CAAC,CAAC,EAAE,CAAC,CAAC,GAAG,SAAS,CAAC,GAAG,SAAS,CAAC,CAAC,CAAC;QACzF,OAAO,cAAc,CAAC,QAAQ,GAAG,KAAK,GAAG,KAAK,CAAC,CAAC;IAClD,CAAC,CAAC;AACJ,CAAC;AAED;;;;GAIG;AACH,MAAM,UAAU,YAAY,CAC1B,QAAoC,EACpC,OAA4B,EAC5B,kBAA0B;IAE1B,MAAM,IAAI,GAAG,QAAQ,CAAC,MAAM,CAAC,CAAC,MAAM,EAAE,EAAE;QACtC,MAAM,KAAK,GAAG,MAAM,CAAC,CAAC,CAAC,CAAC;QACxB,MAAM,IAAI,GAAG,MAAM,CAAC,MAAM,CAAC,MAAM,GAAG,CAAC,CAAC,CAAC;QACvC,IAAI,KAAK,KAAK,SAAS,IAAI,IAAI,KAAK,SAAS;YAAE,OAAO,KAAK,CAAC;QAC5D,IAAI,MAAM,CAAC,MAAM,GAAG,CAAC;YAAE,OAAO,IAAI,CAAC;QACnC,OAAO,IAAI,CAAC,KAAK,CAAC,IAAI,CAAC,CAAC,GAAG,KAAK,CAAC,CAAC,EAAE,IAAI,CAAC,CAAC,GAAG,KAAK,CAAC,CAAC,CAAC,GAAG,CAAC,CAAC;IAC5D,CAAC,CAAC,CAAC;IACH,MAAM,MAAM,GAAG,IAAI,CAAC,GAAG,CAAC,CAAC,MAAM,EAAE,EAAE,CAAC,YAAY,CAAC,MAAM,CAAC,CAAC,CAAC;IAC1D,MAAM,OAAO,GAAG,MAAM,CAAC,GAAG,CAAC,CAAC,KAAK,EAAE,EAAE,CAAC,KAAK,CAAC,eAAe,CAAC,CAAC;IAC7D,MAAM,KAAK,GAAG,OAAO,CAAC,MAAM,CAAC,CAAC,GAAG,EAAE,KAAK,EAAE,EAAE,CAAC,GAAG,GAAG,KAAK,EAAE,CAAC,CAAC,CAAC;IAE7D,IAAI,SAAS,GAAG,CAAC,CAAC;IAClB,MAAM,QAAQ,GAAiB,MAAM,CAAC,GAAG,CAAC,CAAC,KAAK,EAAE,KAAK,EAAE,EAAE;QACzD,MAAM,aAAa,GAAG,KAAK,KAAK,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,SAAS,GAAG,KAAK,CAAC;QAC1D,SAAS,IAAI,OAAO,CAAC,KAAK,CAAW,CAAC;QACtC,MAAM,WAAW,GAAG,KAAK,KAAK,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,CAAC,SAAS,GAAG,KAAK,CAAC;QACxD,OAAO;YACL,KAAK;YACL,OAAO,EAAE,cAAc,CAAC,OAAO,EAAE,KAAK,EAAE,aAAa,EAAE,WAAW,EAAE,kBAAkB,CAAC;SACxF,CAAC;IACJ,CAAC,CAAC,CAAC;IAEH,MAAM,IAAI,GAAG,QAAQ,CAAC,QAAQ,CAAC,MAAM,GAAG,CAAC,CAAC,CAAC;IAC3C,MAAM,WAAW,GAAG,QAAQ,CAAC,QAAQ,CAAC,MAAM,GAAG,CAAC,CAAC,CAAC;IAClD,MAAM,QAAQ,GAAG,IAAI,EAAE,KAAK,CAAC,GAAG,IAAI,WAAW,EAAE,CAAC,WAAW,CAAC,MAAM,GAAG,CAAC,CAAC,IAAI,EAAE,CAAC,EAAE,CAAC,EAAE,CAAC,EAAE,CAAC,EAAE,CAAC;IAC5F,OAAO;QACL,QAAQ;QACR,gBAAgB,EAAE,OAAO;QACzB,cAAc,EAAE,KAAK;QACrB,OAAO,EAAE;YACP,CAAC,EAAE,QAAQ,CAAC,CAAC;YACb,CAAC,EAAE,QAAQ,CAAC,CAAC;YACb,OAAO,EAAE,IAAI,KAAK,SAAS,CAAC,CAAC,CAAC,cAAc,CAAC,kBAAkB,CAAC,CAAC,CAAC,CAAC,IAAI,CAAC,OAAO,CAAC,CAAC,CAAC;SACnF;QACD,eAAe,EAAE,QAAQ,CAAC,MAAM,GAAG,IAAI,CAAC,MAAM;KAC/C,CAAC;AACJ,CAAC"}

package/dist/sim/plant.d.ts

@@ -0,0 +1,59 @@
+import type { DrivePowers, MotionState } from "./follower.js";
+import type { PlantParams } from "./types.js";
+/**
+ * The simulated chassis, matched to the robot repository's headless sim: four
+ * `SimLoop/.../fakehardware/FakeMotor` first-order wheels, `MecanumDrivePlant`'s mounting-sign
+ * bookkeeping (which cancels and so is left out), and `MecanumPoseIntegrator`'s forward kinematics
+ * with its roller-slip factor on the lateral velocity. The powers reach the wheels the way Pedro's
+ * `Mecanum.applyDrive` and `CachedMotor.setPower` write them.
+ *
+ * One deliberate difference, and only with `coast: "natural"` (the `robot` fidelity): when every
+ * wheel is unpowered the chassis loses speed at the measured natural deceleration per axis, the
+ * model Pedro's own `getCoastDisplacement` uses, instead of at the first-order rate. The Gradle
+ * plant uses one rate for both, which `BiobuzzSimConfig.MAX_ACCEL`'s javadoc records as sheding
+ * 387 in/s^2 at zero power where the robot measured 89.
+ */
+export interface PlantState {
+    x: number;
+    /** Field-frame y, inches. */
+    y: number;
+    /** Unwrapped, like `MecanumPoseIntegrator.heading`. */
+    heading: number;
+    /** Wheel speeds as fractions of full speed, [FL, FR, BL, BR]. */
+    speeds: [number, number, number, number];
+    /** The powers the motors were last given, as `CachedMotor` holds them. */
+    powers: [number, number, number, number];
+}
+export declare const copyPlantState: (state: PlantState) => PlantState;
+export declare class MecanumPlant {
+    readonly params: PlantParams;
+    readonly lateralEfficiency: number;
+    state: PlantState;
+    constructor(params: PlantParams, x: number, y: number, heading: number);
+    /**
+     * `Mecanum.applyDrive`: wheel powers, normalised so none exceeds 1, each through
+     * `CachedMotor.setPower`, which skips a change smaller than the threshold unless the sign flips.
+     */
+    drive(powers: DrivePowers): void;
+    /** `Mecanum.stop`. */
+    stop(): void;
+    private setPower;
+    /** The chassis velocity the wheels give right now: body-frame forward, left, and turn rate. */
+    bodyVelocity(): {
+        vx: number;
+        vy: number;
+        omega: number;
+    };
+    /** The true motion state, as a localizer that measures velocity would report it. */
+    motion(): MotionState;
+    /** One tick: the wheels chase their powers, then the pose integrates the chassis velocity. */
+    update(dt: number): void;
+    /**
+     * Every wheel unpowered on a floating drivetrain: each body axis sheds speed at its own measured
+     * rate, forward and strafe from `robot.json`, rotation at the forward rate over the lever arm.
+     * The wheel speeds are then set back from the body velocity, which is exact because the
+     * forward kinematics lose only the mode a mecanum cannot drive.
+     */
+    private coastNaturally;
+}
+//# sourceMappingURL=plant.d.ts.map

package/dist/sim/plant.d.ts.map

@@ -0,0 +1 @@
+{"version":3,"file":"plant.d.ts","sourceRoot":"","sources":["../../src/sim/plant.ts"],"names":[],"mappings":"AAAA,OAAO,KAAK,EAAE,WAAW,EAAE,WAAW,EAAE,MAAM,eAAe,CAAC;AAE9D,OAAO,KAAK,EAAE,WAAW,EAAE,MAAM,YAAY,CAAC;AAE9C;;;;;;;;;;;;GAYG;AAEH,MAAM,WAAW,UAAU;IACzB,CAAC,EAAE,MAAM,CAAC;IACV,6BAA6B;IAC7B,CAAC,EAAE,MAAM,CAAC;IACV,uDAAuD;IACvD,OAAO,EAAE,MAAM,CAAC;IAChB,iEAAiE;IACjE,MAAM,EAAE,CAAC,MAAM,EAAE,MAAM,EAAE,MAAM,EAAE,MAAM,CAAC,CAAC;IACzC,0EAA0E;IAC1E,MAAM,EAAE,CAAC,MAAM,EAAE,MAAM,EAAE,MAAM,EAAE,MAAM,CAAC,CAAC;CAC1C;AAED,eAAO,MAAM,cAAc,GAAI,OAAO,UAAU,KAAG,UAMjD,CAAC;AAEH,qBAAa,YAAY;IACvB,QAAQ,CAAC,MAAM,EAAE,WAAW,CAAC;IAC7B,QAAQ,CAAC,iBAAiB,EAAE,MAAM,CAAC;IACnC,KAAK,EAAE,UAAU,CAAC;gBAEN,MAAM,EAAE,WAAW,EAAE,CAAC,EAAE,MAAM,EAAE,CAAC,EAAE,MAAM,EAAE,OAAO,EAAE,MAAM;IAMtE;;;OAGG;IACH,KAAK,CAAC,MAAM,EAAE,WAAW,GAAG,IAAI;IAUhC,sBAAsB;IACtB,IAAI,IAAI,IAAI;IAIZ,OAAO,CAAC,QAAQ;IAShB,+FAA+F;IAC/F,YAAY,IAAI;QAAE,EAAE,EAAE,MAAM,CAAC;QAAC,EAAE,EAAE,MAAM,CAAC;QAAC,KAAK,EAAE,MAAM,CAAA;KAAE;IAUzD,oFAAoF;IACpF,MAAM,IAAI,WAAW;IAiBrB,8FAA8F;IAC9F,MAAM,CAAC,EAAE,EAAE,MAAM,GAAG,IAAI;IA4BxB;;;;;OAKG;IACH,OAAO,CAAC,cAAc;CAmBvB"}

package/dist/sim/plant.js

@@ -0,0 +1,128 @@
+import { normalizeAngle } from "./pedroPath.js";
+export const copyPlantState = (state) => ({
+    x: state.x,
+    y: state.y,
+    heading: state.heading,
+    speeds: [...state.speeds],
+    powers: [...state.powers],
+});
+export class MecanumPlant {
+    params;
+    lateralEfficiency;
+    state;
+    constructor(params, x, y, heading) {
+        this.params = params;
+        this.lateralEfficiency = params.maxStrafeVelInPerS / params.maxForwardVelInPerS;
+        this.state = { x, y, heading, speeds: [0, 0, 0, 0], powers: [0, 0, 0, 0] };
+    }
+    /**
+     * `Mecanum.applyDrive`: wheel powers, normalised so none exceeds 1, each through
+     * `CachedMotor.setPower`, which skips a change smaller than the threshold unless the sign flips.
+     */
+    drive(powers) {
+        const f = powers.forward;
+        const s = powers.strafe;
+        const t = powers.turn;
+        const wheels = [f - s - t, f + s + t, f + s - t, f - s + t];
+        let max = 1;
+        for (const wheel of wheels)
+            max = Math.max(max, Math.abs(wheel));
+        for (let i = 0; i < 4; i += 1)
+            this.setPower(i, wheels[i] / max);
+    }
+    /** `Mecanum.stop`. */
+    stop() {
+        for (let i = 0; i < 4; i += 1)
+            this.setPower(i, 0);
+    }
+    setPower(wheel, power) {
+        if (!Number.isFinite(power))
+            return;
+        const desired = power < -1 ? -1 : power > 1 ? 1 : power;
+        const last = this.state.powers[wheel];
+        if (Math.abs(last - desired) >= this.params.powerThreshold || Math.sign(last) !== Math.sign(desired)) {
+            this.state.powers[wheel] = desired;
+        }
+    }
+    /** The chassis velocity the wheels give right now: body-frame forward, left, and turn rate. */
+    bodyVelocity() {
+        const [fl, fr, bl, br] = this.state.speeds;
+        const v = this.params.maxForwardVelInPerS;
+        return {
+            vx: (v * (fl + fr + bl + br)) / 4,
+            vy: ((v * (-fl + fr + bl - br)) / 4) * this.lateralEfficiency,
+            omega: (v * (-fl + fr - bl + br)) / (4 * this.params.leverArmIn),
+        };
+    }
+    /** The true motion state, as a localizer that measures velocity would report it. */
+    motion() {
+        const body = this.bodyVelocity();
+        const h = this.state.heading;
+        const cos = Math.cos(h);
+        const sin = Math.sin(h);
+        return {
+            x: this.state.x,
+            y: this.state.y,
+            heading: normalizeAngle(h),
+            vx: body.vx * cos - body.vy * sin,
+            vy: body.vx * sin + body.vy * cos,
+            omega: body.omega,
+            twistX: body.vx,
+            twistY: body.vy,
+        };
+    }
+    /** One tick: the wheels chase their powers, then the pose integrates the chassis velocity. */
+    update(dt) {
+        const p = this.params;
+        const st = this.state;
+        const unpowered = st.powers.every((power) => Math.abs(power) < p.stopSnapFraction);
+        if (p.coast === "natural" && unpowered) {
+            this.coastNaturally(dt);
+        }
+        else {
+            for (let i = 0; i < 4; i += 1) {
+                const power = st.powers[i];
+                let speed = st.speeds[i];
+                speed += (power - speed) * p.wheelResponseRatePerS * dt;
+                if (Math.abs(speed) < p.stopSnapFraction && Math.abs(power) < p.stopSnapFraction)
+                    speed = 0;
+                st.speeds[i] = speed;
+            }
+        }
+        const body = this.bodyVelocity();
+        const cos = Math.cos(st.heading);
+        const sin = Math.sin(st.heading);
+        st.x += (body.vx * cos - body.vy * sin) * dt;
+        st.y += (body.vx * sin + body.vy * cos) * dt;
+        st.heading += body.omega * dt;
+        const half = p.fieldHalfIn;
+        st.x = Math.max(-half, Math.min(half, st.x));
+        st.y = Math.max(-half, Math.min(half, st.y));
+    }
+    /**
+     * Every wheel unpowered on a floating drivetrain: each body axis sheds speed at its own measured
+     * rate, forward and strafe from `robot.json`, rotation at the forward rate over the lever arm.
+     * The wheel speeds are then set back from the body velocity, which is exact because the
+     * forward kinematics lose only the mode a mecanum cannot drive.
+     */
+    coastNaturally(dt) {
+        const p = this.params;
+        const body = this.bodyVelocity();
+        const shed = (value, rate) => {
+            const step = rate * dt;
+            return Math.abs(value) <= step ? 0 : value - Math.sign(value) * step;
+        };
+        const vx = shed(body.vx, p.forwardDecelInPerS2);
+        const vy = shed(body.vy, p.strafeDecelInPerS2);
+        const omega = shed(body.omega, p.forwardDecelInPerS2 / p.leverArmIn);
+        const v = p.maxForwardVelInPerS;
+        const wheelsVy = vy / this.lateralEfficiency;
+        const turn = omega * p.leverArmIn;
+        const speeds = this.state.speeds;
+        speeds[0] = (vx - wheelsVy - turn) / v;
+        speeds[1] = (vx + wheelsVy + turn) / v;
+        speeds[2] = (vx + wheelsVy - turn) / v;
+        speeds[3] = (vx - wheelsVy + turn) / v;
+    }
+}
+//# sourceMappingURL=plant.js.map