mangavalkandClaude Opus 5.5
935526eab5
Simulation - sim/: the unchanged robot firmware compiled into hexapod_sim.dll against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050, RF24 with matching-config radio), a kinematic body model (support plane + planted-foot odometry) that also drives the simulated IMU, and a headless smoke test (sim_smoke). build.ps1 builds and tests. - Hexapod_Simulation_Unity/ folded in (history kept in a bundle outside the repo). Assets/HexapodSim loads the DLL, drives the existing leg rig, and offers joystick / keyboard / robot-serial input plus a test panel. Robot (all found or confirmed in the simulation) - Config.h geometry taken from the Unity model. - IK femur elevation sign, gait stance direction, body pose rotation direction, yaw lever arm. - Stance feet integrated from the current command; gait restarts at mid-stance; heading integrates the applied turn rate, only while walking. - Leg clearance and reach checks (Limits::feetFeasible); walk command and body pose scaled as a whole, pose scale rate-limited, planted feet guarded so the body never snaps or legs collide. - Robot-gyro self-leveling as an integrator. - Heading mode: steering relative to the body; S walks backward. - Standing height from the remote (STANCE_HEIGHT_RANGE_MM). Protocol v3 (reflash both boards) - CmdPacket gains body_height (22 bytes). Remote - Height slider (Nextion hHeight/nHeight, serial [ ]). - Fixed unspecified I2C read order in RemoteIMU. LEGACY/: owner's old Nextion HMI and remote hardware assets. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
148 lines
7.3 KiB
Markdown
148 lines
7.3 KiB
Markdown
# 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
|
||
```
|