| |
| export const RAD = Math.PI / 180; |
| export const clamp = (n: number, a: number, b: number) => Math.max(a, Math.min(b, n)); |
| export const deadzone = (n: number) => Math.abs(n) <= .15 ? 0 : Math.sign(n) * (Math.abs(n) - .15) / .85; |
| export interface Input { axes: number[]; triggers: number[]; shoulders: boolean[]; turbo?: boolean } |
| export const idle = (): Input => ({ axes: [0, 0, 0, 0], triggers: [0, 0], shoulders: [false, false] }); |
| export const neutral = (input: Input) => input.axes.every(n => Math.abs(n) <= .15) && |
| input.triggers.every(n => n <= .15) && !input.shoulders.some(Boolean); |
| export interface Snapshot { head?: number[]; antennas?: number[]; antennas_joint_positions?: number[]; body_yaw?: number; head_joint_positions?: number[] } |
| export interface Target { head: number[]; antennas: number[]; body_yaw: number } |
|
|
| export function matrix(roll: number, pitch: number, yaw: number, x: number, y: number, z: number): number[] { |
| const cr = Math.cos(roll), sr = Math.sin(roll), cp = Math.cos(pitch), sp = Math.sin(pitch), cy = Math.cos(yaw), sy = Math.sin(yaw); |
| return [cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr, x, |
| sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr, y, -sp, cp*sr, cp*cr, z, 0, 0, 0, 1]; |
| } |
|
|
| export class Motion { |
| target: number[]; |
| current: number[]; |
| roll: number; |
| x: number; |
| y: number; |
| constructor(state: Snapshot) { |
| const h = state.head, ant = state.antennas_joint_positions ?? state.antennas; |
| const body = state.body_yaw ?? state.head_joint_positions?.[0]; |
| if (!h || h.length !== 16 || !h.every(Number.isFinite) || !ant || ant.length !== 2 || !ant.every(Number.isFinite) || !Number.isFinite(body)) { |
| throw new Error('Waiting for complete head, body and antenna telemetry.'); |
| } |
| this.roll = Math.atan2(h[9], h[10]); this.x = h[3]; this.y = h[7]; |
| this.current = [Math.atan2(h[4], h[0]) - body!, Math.asin(clamp(-h[8], -1, 1)), h[11], body!, ...ant]; |
| this.target = [...this.current]; |
| } |
| step(input: Input, seconds: number, turbo: boolean): Target { |
| if (input.axes.length !== 4 || input.triggers.length !== 2 || input.shoulders.length !== 2 || |
| ![...input.axes, ...input.triggers, seconds].every(Number.isFinite)) throw new Error('Invalid control input'); |
| const dt = clamp(seconds, 0, .05), multiplier = turbo ? 2 : 1; |
| let yaw = deadzone(input.axes[0]), pitch = deadzone(input.axes[1]); |
| if (Math.abs(yaw) > Math.abs(pitch) * 2) pitch = 0; |
| else if (Math.abs(pitch) > Math.abs(yaw) * 2) yaw = 0; |
| const rates = [-yaw*75*RAD, pitch*60*RAD, -deadzone(input.axes[3])*.025, |
| -deadzone(input.axes[2])*60*RAD, |
| (Number(input.shoulders[1])-deadzone(input.triggers[1]))*120*RAD, |
| (deadzone(input.triggers[0])-Number(input.shoulders[0]))*120*RAD]; |
| this.target = this.target.map((v, i) => v + rates[i]*dt*multiplier); |
| this.target[2] = clamp(this.target[2], -.020, .030); |
| const factor = 1 - .7*Math.abs(this.target[2])/(this.target[2] > 0 ? .030 : .020); |
| const limits = [45*RAD*factor, 20*RAD*factor, 0, 90*RAD, 60*RAD, 60*RAD]; |
| for (const i of [0, 1, 3, 4, 5]) this.target[i] = clamp(this.target[i], -limits[i], limits[i]); |
| const alpha = 1-Math.exp(-dt/(turbo ? .045 : .08)); |
| const maxRates = [90*RAD, 60*RAD, .025, 60*RAD, 120*RAD, 120*RAD]; |
| this.current = this.current.map((v, i) => v + clamp((this.target[i]-v)*alpha, -maxRates[i]*dt*multiplier, maxRates[i]*dt*multiplier)); |
| const [neck, p, z, body, right, left] = this.current; |
| return {head: matrix(this.roll, p, body+neck, this.x, this.y, z), antennas: [right, left], body_yaw: body}; |
| } |
| } |
|
|
| export function readPad(pad: Pick<Gamepad, 'mapping' | 'axes' | 'buttons' | 'connected'>): Input { |
| if (!pad.connected || pad.mapping !== 'standard' || pad.axes.length < 4 || pad.buttons.length < 14) { |
| throw new Error('Use a standard-mapped Xbox/PlayStation gamepad, or choose Touch controls.'); |
| } |
| const axis = (i: number) => clamp(pad.axes[i], -1, 1); |
| const pressed = (i: number) => pad.buttons[i].pressed; |
| return { axes: [0,1,2,3].map(axis), triggers: [6,7].map(i => clamp(pad.buttons[i].value, 0, 1)), |
| shoulders: [pressed(4), pressed(5)], turbo: pressed(12) ? true : pressed(13) ? false : undefined }; |
| } |
|
|
| export class Gate { |
| active = false; |
| reason = 'Paused — choose controls, then press Start.'; |
| arm(input: Input, now: number, lastState: number, connected: boolean) { |
| if (!connected || now - lastState > 2500) throw new Error('Waiting for a fresh robot connection.'); |
| if (!neutral(input)) throw new Error('Center both sticks and release triggers/shoulders before starting.'); |
| this.active = true; this.reason = 'Live'; |
| } |
| pause(reason: string) { this.active = false; this.reason = reason; } |
| } |
|
|
