hexapod-robot/2026-current/robot

Hexapod robot firmware (Teensy 4.0)

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 repo root (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; Servo ships with the Teensy framework.

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.
• SERVO_PINS — must be PWM-capable pins on your Teensy 4.0 wiring
• SERVO_US_AT_0DEG / SERVO_US_PER_DEG — calibrate per servo, they vary batch to batch
• RADIO_CE_PIN / RADIO_CSN_PIN — your nRF24 wiring. Address, channel, data rate, PA level and payload size are in shared/HexapodProtocol so they can't drift from the remote.
• Pin conflict: SERVO_PINS leg 3 uses pins 11 and 12, which are the Teensy 4.0's SPI MOSI/MISO that the nRF24 needs. Move those two servos before wiring the radio.

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. ServoDriver uses one SERVO_US_AT_0DEG / SERVO_US_PER_DEG for all 18 servos, so servo center = joint angle 0 (tibia straight). On this build the knee servo center is the tibia at 90° to the femur, and the left side is mirrored (per the old Unity/servo code). That needs a per-joint offset and direction before the real robot will stand.

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