hexapod-robot/2026-current/robot/README.md

Hexapod robot firmware (Teensy 3.6)

This is the robot-side firmware. It is fully functional on its own (driven by the serial test harness below) and receives commands from the remote (../remote/) over nRF24L01. The packet format lives in ../shared/HexapodProtocol/HexapodProtocol.h — the single definition both sketches compile against.

Building

From the 2026-current folder (PlatformIO): pio run -e robot, flash with pio run -e robot -t upload, monitor with pio device monitor -e robot. RF24 is pulled in automatically via lib_deps. Servos are driven by the Teensy's hardware PWM directly, so no servo library is needed.

Arduino IDE also works if you copy shared/HexapodProtocol/ into your sketchbook libraries/ folder and install RF24 by TMRh20.

Before you trust this on the floor

The leg geometry in Config.h is taken from the Unity model (Hexapod_Simulation_Unity, 1 unit = 10 mm). Everything else there is still a placeholder. Check all of it against the real robot:

• LEG_MOUNT_ANGLE_DEG / LEG_MOUNT_X_MM / LEG_MOUNT_Y_MM — where each coxa pivot actually sits on your chassis, and which way it points
• COXA_LEN_MM / FEMUR_LEN_MM / TIBIA_LEN_MM — your link lengths
• COXA_MIN_DEG..TIBIA_MAX_DEG — your servos' real safe range of motion
• STANCE_RADIUS_MM / STANCE_HEIGHT_MM — how far out/down you want the resting foot position
• LEG_CLEARANCE_MM: how close neighbouring legs may get. Walking speed and body pose are scaled down to keep it, so set it to about one leg's width.
• SERVOS: one entry per servo with pin, pulse at 0°, signed µs per degree (sign = direction) and pulse limits. It's prefilled from the old robot sketch (../../2019-teensy-nrf24/Hexapod_Teensy/config.h): pins, ranges, trims, and directions from the old Unity servo mapping. Check every joint's direction on the bench.
• Before powering up: re-mount the rear-left and front-left coxa servo horns 90° so those legs point straight out at servo center. The old code needed ±90° offsets there, which left the servos at the end of their travel.
• RADIO_CE_PIN / RADIO_CSN_PIN / RADIO_SCK_PIN: 39 / 15 / 14, as in the old sketch. Address, channel, data rate, PA level and payload size are in shared/HexapodProtocol so they can't drift from the remote.

Bench-check checklist (do this before the first stand-up)

These are sign conventions that depend on which way your hardware is physically mounted, and the code has a best-guess default that may be wrong for your build:

1. IK knee direction (Kinematics.cpp). The femur elevation sign bug (the foot was solved above the hip) is fixed and verified in the simulation: the stance pose comes out femur −1.2°, tibia −98.3°. Still command one leg to a few known foot positions on the bench and check the knee bends the way the sim shows.
2. BodyPose roll/pitch sign (BodyPose.cpp). Fixed in the simulation: the pose was applied as the forward rotation, which inverted roll, pitch and yaw. On the bench, command a small roll/pitch and verify the body leans the way the protocol says (+roll = right side down, +pitch = nose down).
3. RobotIMU register map (RobotIMU.cpp) — written for an MPU6050-style chip (I2C addr 0x68). If your gyro is a different part, its register addresses and scale factors will differ; swap them in.
4. Sit-pose assumption — the built-in sit pose in robot.ino assumes all six feet stay grounded and simply lowers the body (a 0.4x height blend). If your chassis is meant to rest its belly on the ground instead, this needs a different, sequenced approach (fold legs up rather than just lowering).
5. Gait travel direction (GaitEngine.cpp). Fixed: stance now sweeps each foot opposite to the commanded motion. The smoke test checks that +vx moves the body forward and +yaw_rate turns it left (CCW). On the bench, confirm w walks forward and z turns left.
6. Per-servo calibration (Config.h SERVOS). Arm the robot, stand it on a box, and move one joint at a time (or watch the sim next to it): each servo must turn the way the sim shows. To flip a direction, flip that entry's dir. Also check that a femur moves the commanded angle: its old range gives only ≈6.7 µs/° against ≈10.4 for coxa and tibia.

Testing in the simulation

This exact code also compiles for the PC and runs in the Unity model. See ../sim/README.md. sim/build/sim_smoke.exe is a quick headless check after every change.

Testing without a remote yet

robot.ino has #define TEST_SERIAL_INPUT enabled by default. With that on, open the Arduino Serial Monitor (115200 baud, no line ending needed — single keystrokes) and use:

Key	Action
a	toggle armed
s	toggle sit / stand
w / x	walk forward / backward (vx ±)
q / e	strafe left / right (vy ±)
z / c	turn left / right (yaw_rate ±)
g	toggle gait: tripod ↔ wave
m	toggle walk mode: direct (A) ↔ heading-hold (B)
space	zero all velocity (stop walking, stay in current state)

Each keystroke injects a CmdPacket through Comms::injectPacket() — the exact same path a real remote's received radio packets go through, so the rest of the pipeline (mapper → state machine → gait → pose → IK → servos) runs identically to how it will with real hardware. Once you have a real remote transmitting, just remove the #define TEST_SERIAL_INPUT line (or leave it — injected packets simply won't happen if you don't type).

Arm (a), then stand (s), before trying to walk — the state machine enforces DISARMED → SIT → STAND → WALKING in order and will otherwise ignore walk commands.

Link with the remote

• CmdPacket and the FLAG_* bits in shared/HexapodProtocol are the exact wire format. Both radios set a static payload size of sizeof(CmdPacket), so a robot and remote built from different protocol versions won't receive each other at all (and the robot fails safe).
• Failsafe: if no packet arrives for LINK_TIMEOUT_SEC, Comms::linkOk() goes false and the main loop falls back to a zeroed CmdPacket, which disarms the robot.
• The right stick's roll/pitch/yaw (or X/Y/Z) always apply. The remote's handset tilt arrives in its own remote_roll / remote_pitch fields and is added on top only while FLAG_REMOTE_GYRO is set, capped at MAX_TILT_DEG total (InputMapper). FLAG_ROBOT_GYRO independently enables the robot's own IMU stabilization.
• FLAG_RSTICK_XLATE switches the right stick between rotation-axes and translation-axes interpretation, and FLAG_MODE_B switches between the two walk modes, all inside the existing InputMapper.

File layout

robot/
├── robot.ino          - main sketch: setup/loop, state wiring, serial test harness
├── Config.h           - all placeholder constants (edit this first)
├── SmoothedValue.h     - generic rate-limited setpoint smoother
├── Kinematics.h/.cpp   - per-leg 3-DOF analytical IK
├── GaitEngine.h/.cpp   - tripod/wave gait, per-leg phase, dynamic gait swap
├── BodyPose.h/.cpp     - 6-axis body pose composition (feedforward + IMU correction)
├── RobotIMU.h/.cpp     - gyro/accel read + complementary filter
├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend
├── InputMapper.h/.cpp  - CmdPacket -> twist/pose/mode decoding
├── Limits.h/.cpp       - workspace clamp, joint rate limit, stance polygon check
├── Comms.h/.cpp        - nRF24L01 receive (packet definition is in shared/HexapodProtocol)
└── ServoDriver.h/.cpp  - servo attach/relax/write