# 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
```