hexapod-robot/2026-current/robot

Hexapod robot firmware (Teensy 4.0)

This is the robot-side firmware only. It is fully functional on its own (driven by the serial test harness below) and is already wired to receive commands from a future remote over nRF24L01 — the packet format in Comms.h is the contract the remote sketch must match.

Required libraries

Install via Arduino Library Manager (or Teensyduino's manager):

• RF24 by TMRh20 — nRF24L01 radio driver
• Servo — bundled with Teensyduino, nothing to install

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_ADDRESS / channel (set in Comms.cpp) — must match the remote once it exists

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).

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
q / e	strafe left / right
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.

What's ready for the remote

• Comms.h's CmdPacket struct and FLAG_* bit constants are the exact wire format the remote must send (as an RF24 payload to RADIO_ADDRESS on the same channel/data-rate/PA-level set in Comms::begin()).
• 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.
• FLAG_REMOTE_GYRO / FLAG_ROBOT_GYRO independently gate whether the remote's tilt feeds into body pose and whether the robot's own IMU stabilizes it — matching the touchscreen toggles you described.
• 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/flags definition
└── ServoDriver.h/.cpp  - servo attach/relax/write