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

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

Everything in Config.h is a placeholder for a generic small hexapod. Measure your own robot and edit:

• 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
• 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 elbow/tibia direction (Kinematics.cpp) — command one leg to a few known foot positions by hand (e.g. via the serial harness) and watch whether the knee bends the way you expect. Flip the sign noted in the comment on tibia_rad if it bends backwards.
2. BodyPose roll/pitch sign (BodyPose.cpp) — command a small roll/pitch offset and verify the body leans the direction you asked for, not the opposite.
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) — the protocol defines +vx = forward, +vy = left, +yaw_rate = turn left (CCW). As written, the stance phase sweeps each foot from neutral - half_stride to neutral + half_stride, i.e. the foot moves with the commanded direction, which pushes the body the opposite way. Verify with w on the bench; if the robot backs up, swap start/end in GaitEngine::update (fixes translation and yaw together). The z/c harness keys were written against the current behavior — recheck them after that fix.

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	yaw left / right
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