| import test from 'node:test'; |
| import assert from 'node:assert/strict'; |
| import { Gate, Motion, RAD, deadzone, idle, matrix, readPad, type Snapshot } from './control.js'; |
| const state = (): Snapshot => ({head: matrix(0,0,0,0,0,0), antennas: [-.17,.17], body_yaw: 0}); |
|
|
| test('first command preserves measured pose and SDK antenna order', () => { |
| const snapshot = {head: matrix(.1,.1,.5,.002,.003,.008), antennas: [-.3,.4], body_yaw: .3}; |
| const result = new Motion(snapshot).step(idle(),0,false); |
| result.head.forEach((n,i) => assert.ok(Math.abs(n-snapshot.head[i]) < 1e-10)); |
| assert.deepEqual(result.antennas, [-.3,.4]); assert.equal(result.body_yaw,.3); |
| }); |
| test('incomplete/non-finite telemetry cannot arm a motion model', () => { |
| assert.throws(() => new Motion({head: [0]})); |
| assert.throws(() => new Motion({...state(), body_yaw: NaN})); |
| }); |
| test('elapsed-time target integration is frame-rate independent', () => { |
| const results = [30,60,120].map(hz => { |
| const motion = new Motion(state()); const input = idle(); input.axes[0] = .4; |
| for(let i=0;i<hz;i++) motion.step(input,1/hz,false); |
| return motion.target[0]; |
| }); |
| results.forEach(n => assert.ok(Math.abs(n-results[0]) < 1e-9)); |
| }); |
| test('head follows body while keeping relative look yaw', () => { |
| const motion = new Motion(state()), input = idle(); input.axes[2] = 1; |
| const result = motion.step(input,.02,false); |
| assert.ok(result.body_yaw < 0); |
| assert.ok(Math.abs(Math.atan2(result.head[4],result.head[0])-result.body_yaw) < 1e-9); |
| }); |
| test('pitch direction matches Pollen: stick up means negative pitch', () => { |
| const motion = new Motion(state()), input = idle(); input.axes[1] = -1; |
| motion.step(input,.02,false); assert.ok(motion.target[1] < 0); |
| }); |
| test('left and right antennas remain independently expressive', () => { |
| const motion = new Motion(state()), input = idle(); input.triggers[0] = 1; |
| motion.step(input,.02,false); |
| assert.equal(motion.target[4],-.17); assert.ok(motion.target[5] > .17); |
| input.triggers[0] = 0; input.shoulders[1] = true; |
| motion.step(input,.02,false); assert.ok(motion.target[4] > -.17); |
| }); |
| test('Turbo doubles antenna integration without enlarging limits', () => { |
| const a = new Motion(state()), b = new Motion(state()), input = idle(); input.triggers[0] = 1; |
| a.step(input,.02,false); b.step(input,.02,true); |
| assert.ok(Math.abs((b.target[5]-.17)-2*(a.target[5]-.17)) < 1e-9); |
| for(let i=0;i<2000;i++) b.step(input,.02,true); |
| assert.ok(b.current[5] <= 60*RAD); |
| }); |
| test('stalls are capped and all channels remain bounded', () => { |
| const motion = new Motion(state()), input = idle(); input.axes = [1,1,1,1]; input.triggers = [1,1]; |
| motion.step(input,100,false); assert.ok(Math.abs(motion.target[3]) <= 60*RAD*.05+1e-9); |
| for(let i=0;i<2000;i++) motion.step(input,.02,true); |
| assert.ok(Math.abs(motion.current[0]) < 65*RAD); |
| assert.ok(Math.abs(motion.current[3]) <= 90*RAD); |
| assert.ok(motion.current[2] >= -.02 && motion.current[2] <= .03); |
| }); |
| test('deadzone is continuous and prevents trigger drift', () => { |
| assert.equal(deadzone(.1),0); assert.ok(deadzone(.150001) < .00001); |
| const motion = new Motion(state()), input = idle(); input.triggers = [.05,.05]; |
| motion.step(input,.02,false); assert.deepEqual(motion.target.slice(4),[-.17,.17]); |
| }); |
| test('arm requires fresh telemetry, connection, and centered controls', () => { |
| const gate = new Gate(); |
| assert.throws(() => gate.arm(idle(),4000,0,true)); |
| assert.throws(() => gate.arm(idle(),100,100,false)); |
| const held = idle(); held.triggers[1] = 1; |
| assert.throws(() => gate.arm(held,100,100,true)); assert.equal(gate.active,false); |
| gate.arm(idle(),100,100,true); assert.equal(gate.active,true); |
| gate.pause('Controller lost'); assert.equal(gate.active,false); |
| assert.equal(gate.reason,'Controller lost'); |
| }); |
| test('standard Gamepad mapping retains sticks, antennas and D-pad', () => { |
| const buttons = Array.from({length: 16}, () => ({pressed:false,value:0,touched:false})); |
| buttons[6] = {pressed:true,value:.75,touched:true}; buttons[12].pressed = true; |
| const input = readPad({mapping:'standard',connected:true,axes:[.2,-.3,.4,-.5],buttons}); |
| assert.deepEqual(input.axes,[.2,-.3,.4,-.5]); assert.deepEqual(input.triggers,[.75,0]); assert.equal(input.turbo,true); |
| assert.throws(() => readPad({mapping:'',connected:true,axes:[0,0,0,0],buttons})); |
| }); |
| test('invalid samples cannot become robot targets', () => { |
| const input = idle(); input.axes[0] = NaN; |
| assert.throws(() => new Motion(state()).step(input,.02,false)); |
| }); |
|
|
