modify stage 1 and add stage 3

[minigithubo]

Summary

This PR adds Stage 2 integration and Stage 3 GPS output to the existing Stage 1 physics simulator running on the Router Arduino Due.

Changes

Stage 2 (PC → Router): ASCII command input

• Added readScriptCommand() to receive ASCII drive commands from PC simulator over USB Serial
• Format: "CANID,nbytes,speed_cmPs,brake,mode,angle_tenths"
• Example: "350,6,1500,0,1,-21" = 1.5 m/s, no brake, mode 1, -2.1° left turn
• Supports comment lines starting with #
• Supports optional execution time prefix: "1000,350,6,1500,0,1,-21"
• Added debug print after parsing: CMD: speed=X brake=X mode=X angle=X
• When a valid 0x350 command is received, useScriptCommand flag is set and speed/angle are applied directly instead of using the fixed throttle model

Stage 3 (Router → PC): Binary GPS output

• Added sendOrigin() - sends binary 0x251 packet once at startup with UW Bothell GPS origin (47.760934, -122.189963)
• Added sendPosition() - sends binary 0x4C0 packet every 100ms with east_cm and north_cm
• Binary packet format per Elcano CAN wiki: 2-byte header + 8 data bytes

Bug fix: heading update in updatePosition()

• Changed heading += angle / 100 to heading += angle / 10
• Previous divisor of 100 caused nearly invisible curvature (-250 tenths → only -2 tenths/loop change)
• New divisor of 10 produces correct curved trajectory

Tested

• ASCII command parsing verified via CMD debug print in Serial Monitor
• Heading changes continuously when angle != 0
• X/Y position follows curved trajectory
• Live 0x4C0 binary telemetry successfully decoded by Advait's Python pygame simulator
• Bidirectional USB Serial communication confirmed working

Reference

https://www.elcanoproject.org/wiki/Communication