feat(rover): add drive profiles and leased remote command channel

2026-09-25 16:37:56 +03:00
parent 53818230f9
commit 63cbb08ea0
18 changed files with 1845 additions and 0 deletions
@@ -0,0 +1,72 @@
+/** Executable profile proposal. Pure calculations; no hardware or transport. */
+export interface ControlProfile {
+  schema: 'missioncore.rover-control/v1';
+  revision: number;
+  mode: 'tank' | 'arcade';
+  stick: 'left' | 'right';
+  deadband: number;
+  response: 'linear' | 'squared';
+  outputScale: number;
+}
+export interface Axes { leftY: number; rightY: number; leftX: number; rightX: number }
+export interface Sides { left: number; right: number }
+export interface MotorBinding { uuid: string; side: 'left' | 'right'; forwardSign: 1 | -1 }
+
+export const defaultProfile: Readonly<ControlProfile> = Object.freeze({
+  schema: 'missioncore.rover-control/v1', revision: 0, mode: 'tank', stick: 'right',
+  deadband: 0.15, response: 'linear', outputScale: 1,
+});
+export function bounded(value: unknown, min: number, max: number): value is number {
+  return typeof value === 'number' && Number.isFinite(value) && value >= min && value <= max;
+}
+export function parseProfile(value: unknown): ControlProfile {
+  if (!value || typeof value !== 'object' || Array.isArray(value)) throw Error('Invalid profile');
+  const p = value as ControlProfile;
+  if (Object.keys(p).sort().join() !== Object.keys(defaultProfile).sort().join()
+      || p.schema !== defaultProfile.schema || !Number.isSafeInteger(p.revision) || p.revision < 0
+      || !['tank', 'arcade'].includes(p.mode) || !['left', 'right'].includes(p.stick)
+      || !['linear', 'squared'].includes(p.response) || !bounded(p.deadband, 0, 0.5)
+      || !bounded(p.outputScale, 0, 1)) throw Error('Invalid profile');
+  return {...p};
+}
+export function axisKeys(profile: ControlProfile): (keyof Axes)[] {
+  return profile.mode === 'tank' ? ['leftY', 'rightY']
+    : profile.stick === 'right' ? ['rightY', 'rightX'] : ['leftY', 'leftX'];
+}
+export function shapeAxis(value: number, profile: ControlProfile): number {
+  if (!bounded(value, -1, 1)) throw Error('Invalid axis');
+  const magnitude = Math.max(0, (Math.abs(value) - profile.deadband) / (1 - profile.deadband));
+  return Math.sign(value) * (profile.response === 'squared' ? magnitude * magnitude : magnitude);
+}
+/** +Y = forward, +X = turn right, including while reversing (yaw convention).
+ * Arcade diamond desaturation follows the documented WPILib ArcadeDriveIK
+ * convention, with clockwise steering sign adapted here. Output is normalized
+ * demand, NOT amps, watts, ERPM, physical velocity, or a guaranteed turn radius.
+ */
+export function mix(profile: ControlProfile, axes: Axes): Sides {
+  parseProfile(profile);
+  for (const key of axisKeys(profile)) if (!bounded(axes[key], -1, 1)) throw Error('Missing or invalid axis');
+  let left: number, right: number;
+  if (profile.mode === 'tank') {
+    left = shapeAxis(axes.leftY, profile); right = shapeAxis(axes.rightY, profile);
+  } else {
+    const throttle = shapeAxis(profile.stick === 'right' ? axes.rightY : axes.leftY, profile);
+    const turn = shapeAxis(profile.stick === 'right' ? axes.rightX : axes.leftX, profile);
+    const peak = Math.max(Math.abs(throttle), Math.abs(turn));
+    const scale = peak === 0 ? 0 : peak / (Math.abs(throttle) + Math.abs(turn));
+    left = (throttle + turn) * scale; right = (throttle - turn) * scale;
+  }
+  return {left: left * profile.outputScale || 0, right: right * profile.outputScale || 0};
+}
+/** All members of both sides are required, irrespective of 1x1/2x2/6x6. */
+export function motorDemands(sides: Sides, bindings: readonly MotorBinding[]): Record<string, number> {
+  if (!bounded(sides.left, -1, 1) || !bounded(sides.right, -1, 1)
+      || !bindings.some(b => b.side === 'left') || !bindings.some(b => b.side === 'right')) throw Error('Incomplete drive');
+  const values: Record<string, number> = Object.create(null);
+  for (const b of bindings) {
+    if (!/^[0-9a-f]{24}$/.test(b.uuid) || b.uuid in values || !['left', 'right'].includes(b.side)
+        || ![1, -1].includes(b.forwardSign)) throw Error('Invalid motor binding');
+    values[b.uuid] = sides[b.side] * b.forwardSign || 0;
+  }
+  return values;
+}