2026 remote: tabbed Nextion UI, robot telemetry, servo centre calibration

Protocol v4: CmdPacket grows to 26 bytes (cal_cmd, cal_servo, cal_value);
new 30-byte Telemetry rides back on the nRF24 ack payload (robot state,
IMU, battery, 18 joint angles, packets/s, servo-cal echo). Each end drops
wrong-length packets, so version mismatches still fail closed.

Robot (owner-approved): SERVO_CAL state while disarmed, per-servo centre
trims (+-200 us) in EEPROM, telemetry, battery reading; the IMU estimate
now always runs (leveling still gated by ROBOT GYRO).

Remote: generated HMI (scripts/make_remote_hmi.py) with pages splash,
drive, status, servos, cal, setup, upload; page-aware NextionUI that only
writes the showing page and only what changed; servo calibration and
UPDATE DISPLAY on the touchscreen and serial console; battery module.
The display is on Serial2 (pins 7/8), found on the real remote; the old
sketch's Serial1 was never connected to it.

Sim: RF24 shim supports ack payloads, API version 8
(hxsim_get_ack_payload); sim_smoke checks telemetry and every CAL command.
Unity CmdPacket mirror updated.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed 2026-11-17 14:42:37 +01:00
1 parent 481610c9fd
commit 0b836af8de
40 files changed
+1463 -281

No files matched your search

+67 -33
View File
@@ -1,7 +1,7 @@
# Hexapod: project memory # Hexapod: project memory
Hexapod walking robot plus a custom handheld remote, both Teensy-based and Hexapod walking robot plus a custom handheld remote, both Teensy-based and
linked one-way over nRF24L01 (remote → robot, with hardware auto-ack only). linked over nRF24L01: commands remote → robot, telemetry back in the radio's ack payloads.
A Unity simulation runs the *unchanged* robot firmware as a DLL. This file A Unity simulation runs the *unchanged* robot firmware as a DLL. This file
is the persistent context for working on any part of it. Read it fully is the persistent context for working on any part of it. Read it fully
before changing anything. before changing anything.
@@ -51,7 +51,7 @@ PlatformIO, one `platformio.ini`, two environments:
## Communication contract (`shared/HexapodProtocol/HexapodProtocol.h`) ## Communication contract (`shared/HexapodProtocol/HexapodProtocol.h`)
`CmdPacket`, 22 bytes (static_asserted; protocol v3), sent remote → robot at 100 Hz `CmdPacket`, 26 bytes (static_asserted; protocol v4, `HXP_PROTOCOL_VERSION`), sent remote → robot at 100 Hz
(robot `CONTROL_HZ` is 60): (robot `CONTROL_HZ` is 60):
| Field | Range | Meaning | | Field | Range | Meaning |
@@ -65,6 +65,16 @@ PlatformIO, one `platformio.ini`, two environments:
| `speed_scale` | 0–255 | multiplies vx/vy on the robot | | `speed_scale` | 0–255 | multiplies vx/vy on the robot |
| `flags` | bits | `ARMED, STAND, WALK, MODE_B, RSTICK_XLATE, GAIT_WAVE, REMOTE_GYRO, ROBOT_GYRO` (bits 0–7) | | `flags` | bits | `ARMED, STAND, WALK, MODE_B, RSTICK_XLATE, GAIT_WAVE, REMOTE_GYRO, ROBOT_GYRO` (bits 0–7) |
| `seq` | 0–255 | increments per packet (robot doesn't check it yet) | | `seq` | 0–255 | increments per packet (robot doesn't check it yet) |
| `cal_cmd` | `CAL_*` | servo centre calibration, only while not armed: `CAL_NONE` normal, `HOLD` centre pose, `SET` trim, `SAVE` / `REVERT` EEPROM. States, not events: repeats are harmless |
| `cal_servo`, `cal_value` | 0–17, ±200 µs | selected servo (leg × 3 + joint) and its trim for `CAL_SET` |
`Telemetry`, 30 bytes, robot → remote as the **ack payload** of the remote's
next packet (`Comms::loadAckPayload` after every packet received; no extra
transmissions): robot state (`TSTATE_*`, 4 = SERVO_CAL), status bits
(`TSTAT_*`: IMU ok, gyro on, servos driven, trims dirty / stored), IMU
roll/pitch in 0.1°, battery mV (0 = not measured), the 18 commanded joint
angles as int8 (tibia stored + 90), packets received per second, and the
echo of `cal_servo` with its trim.
- **Axis convention** (ROS REP-103 style): body +X forward, +Y left, +Z up. - **Axis convention** (ROS REP-103 style): body +X forward, +Y left, +Z up.
+roll = right side down, +pitch = nose down, +yaw = CCW. +roll = right side down, +pitch = nose down, +yaw = CCW.
@@ -72,11 +82,15 @@ PlatformIO, one `platformio.ini`, two environments:
`InputMapper` owns the scaling: `MAX_LINEAR_MM_S` 150, `InputMapper` owns the scaling: `MAX_LINEAR_MM_S` 150,
`MAX_TURN_RATE_DEG_S` 90, `MAX_TILT_DEG` 20, `MAX_POSE_OFFSET_MM` 40. `MAX_TURN_RATE_DEG_S` 90, `MAX_TILT_DEG` 20, `MAX_POSE_OFFSET_MM` 40.
- **Radio settings** live in the same header: address `"HXPD1"`, channel - **Radio settings** live in the same header: address `"HXPD1"`, channel
96, 250 kbps, PA LOW, **static payload size = sizeof(CmdPacket)** on both 96, 250 kbps, PA LOW. Ack payloads need dynamic payload lengths
radios. That makes version mismatches fail closed: packets are not (`enableAckPayload()` on both radios), so **each end drops any packet
received, the robot times out, and it disarms. whose length isn't sizeof() of its struct**. That keeps version
- **Changing the protocol means reflashing both boards.** Keep it ≤ 32 mismatches failing closed: packets are ignored, the robot times out, and
bytes and update the `static_assert`. it disarms.
- **Changing the protocol means reflashing both boards.** Keep both
structs ≤ 32 bytes, update the `static_assert`s, bump
`HXP_PROTOCOL_VERSION`, and mirror `CmdPacket` in the Unity
`HexapodSimNative.cs`.
- **Failsafes:** the robot sets `linkOk()` false after `LINK_TIMEOUT_SEC` - **Failsafes:** the robot sets `linkOk()` false after `LINK_TIMEOUT_SEC`
(0.3 s) without packets and then uses a default `CmdPacket` (flags 0 = (0.3 s) without packets and then uses a default `CmdPacket` (flags 0 =
disarmed, servos detached). The remote disarms itself after disarmed, servos detached). The remote disarms itself after
@@ -101,33 +115,41 @@ joint-rate clamp → servos.
| `Kinematics` | per-leg 3-DOF analytic IK, `neutralStance()` | | `Kinematics` | per-leg 3-DOF analytic IK, `neutralStance()` |
| `GaitEngine` | tripod/wave patterns, per-leg phase, pattern swap only at touchdown. `limitTwist()` (called in robot.ino right after InputMapper, so the heading estimate integrates the *achieved* ω) scales the walk command until its stride envelope (`strideEnvelope()`: even/odd legs at opposite stride ends, plus a lifted set) is feasible, so full-rate turning can't make neighbouring legs collide. Stance feet are *integrated* each tick from the current twist (v + ω×r at the foot); swing feet arc from lift-off to the next leading edge. `resetFeet()` on entering WALKING restarts the cycle with grounded legs at mid-stance | | `GaitEngine` | tripod/wave patterns, per-leg phase, pattern swap only at touchdown. `limitTwist()` (called in robot.ino right after InputMapper, so the heading estimate integrates the *achieved* ω) scales the walk command until its stride envelope (`strideEnvelope()`: even/odd legs at opposite stride ends, plus a lifted set) is feasible, so full-rate turning can't make neighbouring legs collide. Stance feet are *integrated* each tick from the current twist (v + ω×r at the foot); swing feet arc from lift-off to the next leading edge. `resetFeet()` on entering WALKING restarts the cycle with grounded legs at mid-stance |
| `BodyPose` | 6-axis body pose (smoothed command + IMU roll/pitch correction), applied to foot targets as the *inverse* rotation, so the body moves as commanded. `fitToReach()` scales the *whole* pose by one factor (binary search on `Limits::feetFeasible`; drops at once, recovers at 1/s) so combined poses never bend individual legs out of a rigid foot pattern or into each other. While walking it fits against the stride envelope, not the current feet, so it doesn't saw with the step cycle | | `BodyPose` | 6-axis body pose (smoothed command + IMU roll/pitch correction), applied to foot targets as the *inverse* rotation, so the body moves as commanded. `fitToReach()` scales the *whole* pose by one factor (binary search on `Limits::feetFeasible`; drops at once, recovers at 1/s) so combined poses never bend individual legs out of a rigid foot pattern or into each other. While walking it fits against the stride envelope, not the current feet, so it doesn't saw with the step cycle |
| `RobotIMU` | MPU6050 complementary filter (α 0.98), gated by `FLAG_ROBOT_GYRO` | | `RobotIMU` | MPU6050 complementary filter (α 0.98). Always estimates (the remote shows it); only the self-leveling in robot.ino is gated by `FLAG_ROBOT_GYRO` |
| `StateMachine` | DISARMED → SIT → STAND → WALKING, sit/stand blend at 1/s. Disarm is instant | | `StateMachine` | DISARMED → SIT → STAND → WALKING, sit/stand blend at 1/s. Disarm is instant |
| `InputMapper` | CmdPacket → Twist + PoseCommand + mode flags + stance height. Mode B = steering **relative to where the body faces right now**: the forward/back stick component sets walking direction (vx sign), the sideways component steers (target = current heading + atan2(vy, |vx|) every tick). So W = straight on, S = straight **back** (not turn around; owner's request), W/S+A = walk while turning left, A alone = turn on the spot while held. On release the target freezes at the current heading (hold). An earlier version froze the reference at the first push, so heading only changed after releasing W; the owner rejected that. Mode A keeps the target = current heading so switching to B never jumps. Forward speed × max(0, cos(heading error)), so big corrections turn in place first. Adds remote tilt on top of stick pose when `FLAG_REMOTE_GYRO`, clamped to `MAX_TILT_DEG` | | `InputMapper` | CmdPacket → Twist + PoseCommand + mode flags + stance height. Mode B = steering **relative to where the body faces right now**: the forward/back stick component sets walking direction (vx sign), the sideways component steers (target = current heading + atan2(vy, |vx|) every tick). So W = straight on, S = straight **back** (not turn around; owner's request), W/S+A = walk while turning left, A alone = turn on the spot while held. On release the target freezes at the current heading (hold). An earlier version froze the reference at the first push, so heading only changed after releasing W; the owner rejected that. Mode A keeps the target = current heading so switching to B never jumps. Forward speed × max(0, cos(heading error)), so big corrections turn in place first. Adds remote tilt on top of stick pose when `FLAG_REMOTE_GYRO`, clamped to `MAX_TILT_DEG` |
| `Limits` | workspace clamp (70 mm horiz / 60 mm vert), joint rate clamp, ≥3-legs-in-stance check (not acted on yet). `feetFeasible()` = every foot in its workspace with valid IK **and** neighbouring legs' segments (coxa/femur/tibia, via `Kinematics::jointPositions`) ≥ `LEG_CLEARANCE_MM` apart. This is the single "physically possible" test used by gait and pose limiting | | `Limits` | workspace clamp (70 mm horiz / 60 mm vert), joint rate clamp, ≥3-legs-in-stance check (not acted on yet). `feetFeasible()` = every foot in its workspace with valid IK **and** neighbouring legs' segments (coxa/femur/tibia, via `Kinematics::jointPositions`) ≥ `LEG_CLEARANCE_MM` apart. This is the single "physically possible" test used by gait and pose limiting |
| `Comms` | nRF24 RX, link timeout, `injectPacket()` for the harness | | `Comms` | nRF24 RX (drops wrong-length packets), link timeout, telemetry as ack payload, packets/s, `injectPacket()` for the harness |
| `ServoDriver` | 18 servos on hardware PWM (50 Hz, 16-bit), per-servo calibration from `Config.h` `SERVOS`, duty 0 = relaxed, skips invalid IK solutions | | `ServoDriver` | 18 servos on hardware PWM (50 Hz, 16-bit), per-servo calibration from `Config.h` `SERVOS` plus a per-servo centre trim (µs, EEPROM record at address 0, loaded in setup), duty 0 = relaxed, skips invalid IK solutions. `calibrate()` = SERVO_CAL: applies a `CAL_*` command and holds `CAL_POSE` (coxa 0, femur 0, tibia −90) |
| `Battery` | battery voltage through a divider; `BATTERY_SENSE_PIN` 255 = not wired yet |
`robot.ino` also runs SERVO_CAL (disarmed + `cal_cmd != CAL_NONE`; leaving
it relaxes the servos) and fills the `Telemetry` every loop
(`sendTelemetry`).
## Remote architecture (`remote/`, Teensy 4.0) ## Remote architecture (`remote/`, Teensy 4.0)
Loop: drain Nextion + USB serial input every pass. On each `TX_HZ` (100) Loop: drain Nextion + USB serial input every pass. On each `TX_HZ` (100)
tick: sticks → IMU → radio link status → RemoteState (smoothers, tick: sticks → IMU → radio link status → battery → RemoteState (smoothers,
link-loss disarm) → PacketBuilder → `radio.write` → Nextion render. link-loss disarm, servo-trim read-back from telemetry) → PacketBuilder →
`radio.write` (+ read the ack payload's telemetry) → Nextion render.
| Module | Role | | Module | Role |
|--------|------| |--------|------|
| `remote.ino` | loop wiring, status print, packet dump | | `remote.ino` | loop wiring, status print, packet dump |
| `Config.h` | **all placeholder pins, calibration defaults, deadzones, rates** | | `Config.h` | **all placeholder pins, calibration defaults, deadzones, rates** |
| `UiTypes.h` | `UiCmd`/`UiEvent` (operator requests), `UiView` (what the display shows) | | `UiTypes.h` | `UiCmd`/`UiEvent` (operator requests), `UiPage` (HMI page ids), `UiView` (what the display shows, incl. robot telemetry) |
| `Sticks` | 6 analog axes: ADC averaging + IIR, piecewise calibration around center, invert, deadzone with rescale, expo → ±1000. Boot-time center capture, sweep calibration saved to EEPROM (magic/version/checksum) | | `Sticks` | 6 analog axes: ADC averaging + IIR, piecewise calibration around center, invert, deadzone with rescale, expo → ±1000. Boot-time center capture, sweep calibration saved to EEPROM (magic/version/checksum) |
| `RemoteIMU` | MPU6050, same complementary filter as the robot, axis remap for mounting, boot gyro-bias capture, "level" zero | | `RemoteIMU` | MPU6050, same complementary filter as the robot, axis remap for mounting, boot gyro-bias capture, "level" zero |
| `RemoteState` | authoritative operator state and interlocks: arm needs centered sticks, disarm clears stand, stand needs armed, calibrate needs disarmed, link-loss auto-disarm. Smooths speed scale and fades remote tilt in/out | | `RemoteState` | authoritative operator state and interlocks: arm needs centered sticks, disarm clears stand, stand needs armed, calibrate needs disarmed, link-loss auto-disarm. Smooths speed scale and fades remote tilt in/out. Servo calibration: only while disarmed and on the CAL page; a newly selected servo is `CAL_HOLD` until its trim has been read back from telemetry, then `CAL_SET`; SAVE/REVERT go out for 300 ms. UPDATE DISPLAY needs a second tap |
| `PacketBuilder` | the inverse of InputMapper: stick frame → body frame per the contract, per-submode right-stick mapping, `FLAG_WALK` with 250 ms linger (twist ignored in mode B), tilt deadzone/scale × fade, zeros motion when disarmed | | `PacketBuilder` | the inverse of InputMapper: stick frame → body frame per the contract, per-submode right-stick mapping, `FLAG_WALK` with 250 ms linger (twist ignored in mode B), tilt deadzone/scale × fade, zeros motion when disarmed |
| `RadioLink` | nRF24 TX, short retries (1, 3), ack-based link status and 32-packet quality % | | `RadioLink` | nRF24 TX, short retries (1, 3), ack-based link status and 32-packet quality %, telemetry from the ack payloads (stale after `TELEMETRY_STALE_MS`) |
| `NextionUI` | library-free Nextion driver on `Serial1`. HMI sends `'#' len cmd [val]` frames via `printh`. Remote pushes state by component name, as `control.<name>` (the owner adds a `control` page with vscope-global components to the old HMI), with diffing, a 2 s full refresh, and a redraw on the Nextion 0x88 "ready" frame. Also writes the old HMI's `t1` status texts (`NEXTION_STATUS_TEXTS`). The old page-1 `b2` sends a standard 0x65 touch frame, which triggers `runUploadPassthrough()`: USB ↔ display bridge for Nextion Editor, following `Serial.baud()`, never returns. Serial key `U` does the same | | `Battery` | handset battery through a divider; `BATTERY_SENSE_PIN` 255 = not wired yet (planned A6) |
| `NextionUI` | library-free Nextion driver on `Serial1`. HMI sends `'#' len cmd [val]` frames via `printh`; every page's Preinitialize sends `'P'` + page id (and `sendme` / 0x66 recovers the page after a remote restart). The remote writes **only the showing page**, by local component name, and only what changed (FNV hash per instruction in a per-page slot cache), 20 Hz (SERVOS 10 Hz), never blocking on a full TX buffer; full refresh every 2 s and on the 0x88 "ready" frame. UPDATE DISPLAY (tapped twice) or serial `U` runs `runUploadPassthrough()`: USB ↔ display bridge for Nextion Editor, following `Serial.baud()`, never returns |
| `SerialConsole` | USB keystrokes that mirror every UI command plus diagnostics | | `SerialConsole` | USB keystrokes that mirror every UI command plus diagnostics |
| `SmoothedValue.h` | copy of the robot's (kept separate so robot/ stays untouched) | | `SmoothedValue.h` | copy of the robot's (kept separate so robot/ stays untouched) |
| `NEXTION.md` | HMI build guide: component names, txt_maxl, touch event code | | `NEXTION.md` | the HMI: pages, command bytes, which components the firmware writes |
| `remote.HMI` | **generated** by `scripts/make_remote_hmi.py` (pages splash, drive, status, servos, cal, setup, upload; tabs + top bar with DISARM). Change the layout in the script, not in the Editor |
Remote stick-frame convention: logical X + = right, Y + = forward, twist + Remote stick-frame convention: logical X + = right, Y + = forward, twist +
= CCW. The mapping into the packet is in `PacketBuilder.cpp` (e.g. = CCW. The mapping into the packet is in `PacketBuilder.cpp` (e.g.
@@ -235,7 +257,7 @@ Robot (`robot/README.md` has detail):
limits now in `Config.h`: coxa limits are **per leg** limits now in `Config.h`: coxa limits are **per leg**
(`COXA_MIN_DEG[i]`/`COXA_MAX_DEG[i]`; all ±60 in the hexagon layout, but in (`COXA_MIN_DEG[i]`/`COXA_MAX_DEG[i]`; all ±60 in the hexagon layout, but in
the old sideways layout a corner leg only swung ~36° towards its mid leg), the old sideways layout a corner leg only swung ~36° towards its mid leg),
and `LEG_CLEARANCE_MM` is 56 (a leg is 55.2 mm wide at the joints). The sim API exports the per-leg coxa limits (version 7). and `LEG_CLEARANCE_MM` is 56 (a leg is 55.2 mm wide at the joints). The sim API exports the per-leg coxa limits (version 7). Version 8 adds `hxsim_get_ack_payload` (the robot's telemetry); sim_smoke checks telemetry and every `CAL_*` command.
- **Fixed (owner-approved):** the `Kinematics.cpp` femur elevation sign - **Fixed (owner-approved):** the `Kinematics.cpp` femur elevation sign
(`atan2(local_z, horiz)`; it used to solve for a foot above the hip, so (`atan2(local_z, horiz)`; it used to solve for a foot above the hip, so
every solution was invalid). Stance pose is now femur −1.2°, tibia every solution was invalid). Stance pose is now femur −1.2°, tibia
@@ -365,7 +387,7 @@ Robot (`robot/README.md` has detail):
Remote (`remote/README.md` has detail): Remote (`remote/README.md` has detail):
- Pins in `remote/Config.h`, taken from the old remote sketch: radio CE 9 / - Pins in `remote/Config.h`, taken from the old remote sketch: radio CE 9 /
CSN 10, sticks A0–A5, Nextion on Serial1 (0/1). The MPU6050 is on CSN 10, sticks A0–A5, Nextion on **Serial2 (7/8)**, confirmed on the real remote. The MPU6050 is on
**Wire2 (25/24, solder pads under the Teensy 4.0)**, since A4/A5 = Wire's SDA/SCL are stick pins; that's **Wire2 (25/24, solder pads under the Teensy 4.0)**, since A4/A5 = Wire's SDA/SCL are stick pins; that's
the owner's choice and needs rewiring the MPU. Which stick axis is on the owner's choice and needs rewiring the MPU. Which stick axis is on
which pin is still unknown. which pin is still unknown.
@@ -373,14 +395,25 @@ Remote (`remote/README.md` has detail):
- `IMU_AXIS_FROM_CHIP` / `IMU_AXIS_SIGN` depend on how the MPU6050 is - `IMU_AXIS_FROM_CHIP` / `IMU_AXIS_SIGN` depend on how the MPU6050 is
mounted in the handset. mounted in the handset.
- The Nextion must have `bauds=115200` in Program.s, and the HMI must match - The Nextion must have `bauds=115200` in Program.s, and the HMI must match
`NEXTION.md`. The owner's old 6-page HMI is in `../2019-teensy-nrf24/remote-nextion/remote.HMI`; `NEXTION.md`. The working HMI is `remote/remote.HMI`, generated by
its component names aren't readable outside Nextion Editor. The compiled `scripts/make_remote_hmi.py` (owner-approved design, 2026-11-17; the old 6-page one in
`.tft` is gitignored. `../2019-teensy-nrf24/remote-nextion/` only lends its splash picture). The display still
runs the old HMI until the new `.tft` is uploaded: use serial key `U` for that first upload,
the old UPLOAD button isn't recognised any more. Edit and render it with the `nxhmi` MCP server
(registered in the repo root `.mcp.json`; code is the git submodule `tools/nextion-hmi-tools`,
run `git submodule update --init` after cloning; CLI `python -m nxhmi` with `PYTHONPATH` =
`tools/nextion-hmi-tools/src`): `info`, `get_page`, `render_page`, `set_attributes`,
`set_event`, `add_component`, `add_page`, `verify`, `build_tft`. Its `.tft` builder is
partial and needs the Nextion firmware files, which aren't in the submodule (they're kept
locally in `D:\Claude\nextion-hmi-tools\firmware`), so compile the upload `.tft` in Nextion
Editor. The compiled `.tft` is gitignored.
- What the old remote sketch (`../2019-teensy-nrf24/remote/`, last ran on a - What the old remote sketch (`../2019-teensy-nrf24/remote/`, last ran on a
Teensy) tells us about the real hardware: Teensy) tells us about the real hardware:
- **Nextion on `Serial1` (pins 0/1)**, driven by the ITEAD Nextion - **Nextion on `Serial2` (pins 7/8)**, driven by the ITEAD Nextion
library, whose `nexInit()` runs at 9600. `remote/Config.h` now uses library (its `nexSerial` is Serial2). The sketch's `Serial1.begin()` and
Serial1 and expects 115200 via `bauds=` in the HMI. its USB ↔ Serial1 passthrough never reached the display: a probe on the
real remote (2026-11-17) got `comok` only on Serial2, at 115200.
`remote/Config.h` uses Serial2 at 115200 (`bauds=` in the HMI).
- The old HMI is a *servo monitor*, not the control UI in `NEXTION.md`: - The old HMI is a *servo monitor*, not the control UI in `NEXTION.md`:
- page 1: `t1` "connected"/"not connected", and `b2` = enter - page 1: `t1` "connected"/"not connected", and `b2` = enter
upload mode; upload mode;
@@ -388,11 +421,12 @@ Remote (`remote/README.md` has detail):
`t1` status; `t1` status;
- page 4: `uploadscreen`. - page 4: `uploadscreen`.
None of the arm/stand/gait/mode/gyro/speed/height controls exist yet. It had none of the arm/stand/gait/mode/gyro/speed/height controls.
The owner chose to add a `control` page per `NEXTION.md`. It was replaced by the generated tabbed HMI (see above).
- `b2` → **Nextion upload passthrough**: shows `uploadscreen`, then - `b2` → **Nextion upload passthrough**: shows `uploadscreen`, then
bridges USB ↔ Serial1 forever, so Nextion Editor can upload a `.tft` bridges USB ↔ the display serial forever, so Nextion Editor can upload a `.tft`
through the remote's USB port. The new firmware keeps this. through the remote's USB port. The new firmware keeps this as SETUP ›
UPDATE DISPLAY (page `upload`).
- Sticks on **A0–A5**, contiguous (`analogInPin + i`). On the Teensy 4.0, - Sticks on **A0–A5**, contiguous (`analogInPin + i`). On the Teensy 4.0,
A4/A5 are pins 18/19 = Wire SDA/SCL, which the MPU6050 needs. The old A4/A5 are pins 18/19 = Wire SDA/SCL, which the MPU6050 needs. The old
code had the MPU code disabled (`if (false)`). Owner's decision: keep code had the MPU code disabled (`if (false)`). Owner's decision: keep
@@ -424,7 +458,7 @@ Remote (`remote/README.md` has detail):
with BENCH comments for anything hardware-dependent, `SmoothedValue` with BENCH comments for anything hardware-dependent, `SmoothedValue`
rate limiting on any setpoint that can jump, and paired `.h/.cpp` per rate limiting on any setpoint that can jump, and paired `.h/.cpp` per
concern. concern.
- Planned or possible next steps: an ack-payload telemetry channel from - Planned or possible next steps: wire the battery dividers (firmware is
robot → remote (state, battery, link) to show real robot state on the ready on both sides, pins still 255) and add a low-battery warning; try
Nextion, battery monitoring on both sides, a `.HMI` file checked into ack-payload telemetry and servo calibration on the real radios (only
`remote/`. simulated so far).
@@ -14,7 +14,10 @@ namespace HexapodSim
public short rx, ry, rz; public short rx, ry, rz;
public short remote_roll, remote_pitch; public short remote_roll, remote_pitch;
public short body_height; // -1000 lowest .. 0 default .. 1000 highest public short body_height; // -1000 lowest .. 0 default .. 1000 highest
public short cal_value; // servo centre calibration (remote only; 0 here)
public byte speed_scale, flags, seq; public byte speed_scale, flags, seq;
public byte cal_cmd; // CAL_NONE (0): the sim's remote never calibrates servos
public byte cal_servo;
public const byte FLAG_ARMED = 1 << 0; public const byte FLAG_ARMED = 1 << 0;
public const byte FLAG_STAND = 1 << 1; public const byte FLAG_STAND = 1 << 1;
@@ -73,7 +76,7 @@ namespace HexapodSim
// "Reload firmware" and the new code runs. Each load is a fresh power-on. // "Reload firmware" and the new code runs. Each load is a fresh power-on.
public sealed class HexapodSimNative : IDisposable public sealed class HexapodSimNative : IDisposable
{ {
public const int ExpectedApiVersion = 7; // HXSIM_API_VERSION public const int ExpectedApiVersion = 8; // HXSIM_API_VERSION
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn(); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn(); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
@@ -88,6 +91,7 @@ namespace HexapodSim
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int AckFn(byte[] bytes, int cap);
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask); [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask);
IntPtr handle_; IntPtr handle_;
@@ -107,6 +111,7 @@ namespace HexapodSim
StatusFn getStatus_; StatusFn getStatus_;
TwoFloatsFn setGroundTilt_; TwoFloatsFn setGroundTilt_;
BodyFn getBody_; BodyFn getBody_;
AckFn getAckPayload_;
public string LoadedFrom { get; private set; } public string LoadedFrom { get; private set; }
@@ -146,6 +151,7 @@ namespace HexapodSim
n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status"); n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status");
n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt"); n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt");
n.getBody_ = n.Fn<BodyFn>("hxsim_get_body"); n.getBody_ = n.Fn<BodyFn>("hxsim_get_body");
n.getAckPayload_ = n.Fn<AckFn>("hxsim_get_ack_payload");
int version = n.apiVersion_(); int version = n.apiVersion_();
if (version != ExpectedApiVersion) if (version != ExpectedApiVersion)
@@ -205,6 +211,10 @@ namespace HexapodSim
readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4]; readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4];
// Kinematic body from the DLL, robot-world frame. The rotation is a // Kinematic body from the DLL, robot-world frame. The rotation is a
// robot-frame quaternion held in a Unity Quaternion for its math only. // robot-frame quaternion held in a Unity Quaternion for its math only.
// The robot's telemetry (HexapodProtocol.h Telemetry, 30 bytes) from the
// last ack payload; returns its length, 0 before the first packet.
public int GetAckPayload(byte[] buf) => getAckPayload_(buf, buf.Length);
public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask) public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask)
{ {
getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask); getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask);
+11
View File
@@ -0,0 +1,11 @@
#include "Battery.h"
#include "Config.h"
void Battery::update(float dt) {
if (BATTERY_SENSE_PIN == 255) return; // not fitted: stays 0
// Sticks set the ADC to ADC_BITS for every analogRead().
float v = analogRead(BATTERY_SENSE_PIN) * (BATTERY_ADC_REF_V / ((1 << ADC_BITS) - 1)) * BATTERY_DIVIDER;
if (!primed_) { volts_ = v; primed_ = true; return; }
float a = constrain(dt / 2.0f, 0.0f, 1.0f); // ~2 s time constant
volts_ += (v - volts_) * a;
}
+13
View File
@@ -0,0 +1,13 @@
#pragma once
#include <Arduino.h>
// Handset battery voltage through a resistor divider (Config.h BATTERY_*), smoothed.
class Battery {
public:
void update(float dt);
uint16_t millivolts() const { return (uint16_t)(volts_ * 1000.0f + 0.5f); } // 0 = not fitted
private:
float volts_ = 0;
bool primed_ = false;
};
+31 -18
View File
@@ -109,24 +109,37 @@ constexpr uint32_t WALK_LINGER_MS = 250;
// (post-deadzone, -1000..1000 scale), so the robot never arms mid-command. // (post-deadzone, -1000..1000 scale), so the robot never arms mid-command.
constexpr int16_t ARM_CENTER_TOLERANCE = 50; constexpr int16_t ARM_CENTER_TOLERANCE = 50;
// ---- Nextion touchscreen on Serial1 (RX1 = pin 0, TX1 = pin 1), as in the old remote ---- // ---- Nextion touchscreen on Serial2 (RX2 = pin 7, TX2 = pin 8) ----
#define NEXTION_SERIAL Serial1 // Found on the real remote (2026-11-17): the display answers on Serial2 at 115200. The old
// BENCH: the old sketch used the ITEAD library, whose nexInit() talks at 9600 // sketch's Serial1.begin() was a red herring; its ITEAD Nextion library used Serial2.
// unless it was modified. This firmware expects 115200: add `bauds=115200` #define NEXTION_SERIAL Serial2
// to the HMI's Program.s (see remote/NEXTION.md) or set 9600 here. // The HMI (remote/remote.HMI) sets this with `bauds=115200` in Program.s.
constexpr uint32_t NEXTION_BAUD = 115200; constexpr uint32_t NEXTION_BAUD = 115200;
// Controls live on this page (remote/NEXTION.md). Its components must have #define NEXTION_UPLOAD_PAGE "upload"
// vscope = global, so the firmware can update them whichever page is showing. // The firmware only writes the page that is showing (each page reports itself
#define NEXTION_CONTROL_PAGE "control" // with a 'P' frame), and only what changed since the last write.
// The old HMI's link-status texts, also vscope = global. Its pages are constexpr uint32_t NEXTION_RENDER_MS = 50; // repaint rate of the showing page
// 0 splashsceen (sic), 1 main, 2 servocal, 3 page0, 4 uploadscreen, constexpr uint32_t NEXTION_SERVOS_RENDER_MS = 100; // SERVOS page: 36 values, so a bit slower
// 5 joysticks; the texts are t1 on page 1 (main) and page 3 (page0).
constexpr const char* NEXTION_STATUS_TEXTS[] = { "main.t1", "page0.t1" };
// The old HMI's "upload" button: page 1, component id 5 (b2), sends the
// standard touch event. It puts the remote into Nextion upload passthrough.
constexpr uint8_t NEXTION_UPLOAD_BUTTON_PAGE = 1;
constexpr uint8_t NEXTION_UPLOAD_BUTTON_ID = 5;
#define NEXTION_UPLOAD_PAGE "uploadscreen"
constexpr uint32_t NEXTION_FULL_REFRESH_MS = 2000; // re-push everything periodically (self-heals after a display reset) constexpr uint32_t NEXTION_FULL_REFRESH_MS = 2000; // re-push everything periodically (self-heals after a display reset)
constexpr uint32_t NEXTION_LINK_REFRESH_MS = 250; // link-quality text update rate // A render is skipped while less than this is free in the TX buffer, so the
// TX loop never blocks on the display.
constexpr int NEXTION_TX_MIN_FREE = 400;
constexpr uint32_t STATUS_MESSAGE_MS = 2500; // how long transient status messages stay up constexpr uint32_t STATUS_MESSAGE_MS = 2500; // how long transient status messages stay up
constexpr uint32_t UPLOAD_CONFIRM_MS = 3000; // UPDATE DISPLAY: second tap within this
// ---- Robot telemetry (ack payloads) ----
constexpr uint32_t TELEMETRY_STALE_MS = 500; // older than this -> the display shows "no data"
// ---- Servo centre calibration (CAL page) ----
constexpr int16_t SERVO_TRIM_STEP_SMALL_US = 1;
constexpr int16_t SERVO_TRIM_STEP_BIG_US = 10;
// SAVE / REVERT go out for this long (the robot acts once; repeats are harmless).
constexpr uint32_t SERVO_CAL_OP_MS = 300;
// ---- Battery voltage (STATUS page) ----
// BENCH: not built yet. Planned: the 18650's + through a 100k/100k divider
// into A6 (pin 20) with 100 nF to GND (see README.md "Planned"). Then set
// the pin to A6. 255 = not fitted: the display shows "--".
constexpr uint8_t BATTERY_SENSE_PIN = 255;
constexpr float BATTERY_DIVIDER = 2.0f; // battery V / pin V
constexpr float BATTERY_ADC_REF_V = 3.3f;
+84 -98
View File
@@ -1,112 +1,98 @@
# Nextion HMI build guide # Nextion HMI
Work in `remote.HMI` in this folder (a copy of the old servo monitor, `remote.HMI` in this folder is the touchscreen project (NX4832T035, 480×320).
`../../2019-teensy-nrf24/remote-nextion/remote.HMI`, re-saved by a newer It's **generated** by `../scripts/make_remote_hmi.py`: change the layout
Nextion Editor) and **add one page named `control`** with the components below. there and re-run it, rather than editing the `.HMI` in Nextion Editor (an
Editor edit is lost on the next run). Then open `remote.HMI` in Nextion
The old HMI's pages: Editor, compile, and upload the `.tft`: SETUP › UPDATE DISPLAY (tap twice)
or serial key `U` puts the remote in USB passthrough for the upload.
| # | Name | Contents |
|---|------|----------|
| 0 | `splashsceen` (sic) | start page |
| 1 | `main` | `t1` link status, `b2` (id 5) upload mode |
| 2 | `servocal` | servo calibration (not used by this firmware) |
| 3 | `page0` | servo sliders `h0`–`h17`, `t1` link status. Note the name: `page page0` opens this page, `page 0` (a number) opens the splash |
| 4 | `uploadscreen` | shown during upload mode |
| 5 | `joysticks` | not used by this firmware |
Your existing pages keep working:
| Existing | What the firmware does with it |
|----------|--------------------------------|
| page 1 `t1`, page 3 `t1` | writes `connected` / `not connected`, as the old firmware did, as `main.t1` and `page0.t1` (`NEXTION_STATUS_TEXTS` in `Config.h`). Set both to **vscope = global** |
| page 1 `b2` (id 5) | **upload mode**: keep "Send Component ID" ticked on it. The remote shows `uploadscreen` and passes USB straight through to the display, so Nextion Editor can upload over the remote's USB port. Power-cycle to leave. Serial key `U` does the same |
| page 4 `uploadscreen` | shown on entering upload mode |
| page 3 sliders `h0`–`h17` | not driven yet: the robot doesn't send servo positions back (that needs the planned telemetry channel) |
The firmware drives the display by **component name**, and the display
talks back with small custom frames sent from each button's touch event.
Layout, fonts and colors are up to you. The names and event code must
match exactly.
**Set every component on `control` to vscope = global.** The firmware
writes them as `control.<name>` and keeps them current whichever page is
showing. A local component on another page can't be written from outside,
and with `bkcmd=0` the display ignores those writes silently.
The remote is authoritative. Use **plain Button** components (not
Dual-state buttons) and don't toggle anything locally in the HMI. The
remote repaints every label and color from its own state, including a
full redraw every 2 s and whenever the display reboots.
## Program.s / page setup
In **Program.s**, before `page 0`, add:
``` ```
bauds=115200 python scripts\make_remote_hmi.py (from 2026-current)
``` ```
This sets the display to the `NEXTION_BAUD` in `remote/Config.h`. The It needs the `tools/nextion-hmi-tools` submodule, Pillow, DejaVu Sans Mono
display ships at 9600, and `bauds=` persists across power cycles. The old (found in matplotlib, or `--font-dir`), and the old remote HMI
firmware used the ITEAD library at 9600, so your HMI probably runs at 9600 (`../../2019-teensy-nrf24/remote-nextion/remote.HMI`, read only) for the
today. splash picture. To look at a page without the Editor, use the `nxhmi` MCP
server's `render_page`.
Name the new page `control`. The firmware never changes pages, except ## Pages
into `uploadscreen` for upload mode. Add buttons on your other pages to
switch to it (`page control`).
## Components | id | Page | Contents |
|----|------|----------|
| 0 | `splash` | spider picture, 1.5 s, then `drive` |
| 1 | `drive` | ARM, SIT/STAND, gait, mode, right-stick mode, both gyros, speed + height sliders, messages |
| 2 | `status` | remote and robot IMU roll/pitch, the 6 stick axes, both batteries, radio |
| 3 | `servos` | all 18 joint angles (robot telemetry), laid out as a top view of the robot |
| 4 | `cal` | servo centre calibration: START/STOP, leg and joint, trim ±1/±10 µs, ZERO, REVERT, SAVE |
| 5 | `setup` | stick calibration, LEVEL REMOTE, UPDATE DISPLAY, backlight, versions |
| 6 | `upload` | shown during display update mode |
Text components need **txt_maxl** at least as large as the longest string Pages 1–5 share a frame: a top bar (`tLink` link quality, `tState` robot
the firmware writes. Button bounds here allow some slack. state + battery, red `bKill` DISARM) and tab buttons `bTab0`–`bTab4` that
switch pages locally (`page drive`, ...). The page order is the page id, and
must match `UiPage` in `UiTypes.h`.
| Name | Type | txt_maxl | Firmware writes | ## How the firmware talks to it
|----------|---------|----------|----------------------------------------------|
| `bArm` | Button | 10 | `ARMED` / `DISARMED`, bco red / gray |
| `bKill` | Button | – | nothing (static "DISARM" label, make it red) |
| `bStand` | Button | 6 | `STAND` / `SIT`, bco green / gray |
| `bGait` | Button | 8 | `TRIPOD` / `WAVE` |
| `bMode` | Button | 8 | `DIRECT` / `HEADING` |
| `bRs` | Button | 10 | `R: ROTATE` / `R: MOVE` |
| `bRGyro` | Button | – | bco green / gray (label it "REMOTE GYRO") |
| `bBGyro` | Button | – | bco green / gray (label it "ROBOT GYRO") |
| `hSpeed` | Slider | – | `val` 0..255 (set **maxval = 255**) |
| `nSpeed` | Number | – | `val` 0..100 (speed %) |
| `hHeight`| Slider | – | `val` 0..200, 100 = default (set **maxval = 200**) |
| `nHeight`| Number | – | `val` -100..100 (height offset %) |
| `bCal` | Button | 9 | `CAL` / `SAVE CAL`, bco orange while active |
| `bCalX` | Button | – | nothing (static "CANCEL CAL") |
| `bZero` | Button | – | nothing (static "LEVEL") |
| `tLink` | Text | 10 | `LINK 97%` / `NO LINK`, pco green / red |
| `tMsg` | Text | 40 | status / rejection messages |
## Touch events - **Display → remote:** every touch sends a custom frame with `printh`:
`'#'` (0x23), length, command byte, optional value byte. "Send Component
ID" is off everywhere.
- **Page tracking:** each page's Preinitialize event sends `'P'` + its page
id, so the remote always knows which page is showing. After a remote
restart it asks with `sendme`.
- **Remote → display:** the remote writes only the showing page, by local
component name (no `vscope = global`, no display RAM spent), and only
what changed (`NextionUI` hashes every instruction). Everything is
re-sent every 2 s and after the display restarts (0x88), so a dropped
byte heals itself. Render rate is 20 Hz (SERVOS 10 Hz), and a render is
skipped rather than blocking when the TX buffer is full.
- **The remote is authoritative:** plain buttons only, nothing toggles
locally. Exceptions that are purely local: the tabs, and the backlight
slider (`dim` / `dims`).
- `Program.s` sets `bauds=115200` (= `NEXTION_BAUD` in `Config.h`), stored in the display.
Put this in each component's **Touch Release Event** (the Speed slider ## Commands (display → remote)
uses **Touch Move** as well so the value tracks your finger). Frame format:
`'#'` (0x23), length, command byte, optional value byte.
| Component | Event code | Command | | Component | Event code | Command |
|-----------|------------------------------------------|--------------------------------| |-----------|-----------|---------|
| `bArm` | `printh 23 01 41` | `A` toggle arm (needs sticks centered) | | every page | `printh 23 02 50 <id>` (Preinitialize) | `P` page shown |
| `bKill` | `printh 23 01 4B` | `K` disarm, always accepted | | `bKill` | `printh 23 01 4B` | `K` disarm, always accepted |
| `bStand` | `printh 23 01 53` | `S` toggle sit/stand | | `bArm` | `printh 23 01 41` | `A` toggle arm (needs sticks centered) |
| `bGait` | `printh 23 01 47` | `G` tripod ↔ wave | | `bStand` | `printh 23 01 53` | `S` toggle sit/stand |
| `bMode` | `printh 23 01 4D` | `M` direct ↔ heading-hold | | `bGait` | `printh 23 01 47` | `G` tripod ↔ wave |
| `bRs` | `printh 23 01 58` | `X` right stick rotate ↔ translate | | `bMode` | `printh 23 01 4D` | `M` direct ↔ heading-hold |
| `bRGyro` | `printh 23 01 52` | `R` remote-gyro tilt on/off | | `bRs` | `printh 23 01 58` | `X` right stick rotate ↔ translate |
| `bBGyro` | `printh 23 01 42` | `B` robot self-stabilize on/off | | `bRGyro` | `printh 23 01 52` | `R` remote-gyro tilt on/off |
| `hSpeed` | `printh 23 02 56`<br>`prints hSpeed.val,1` | `V` + speed byte | | `bBGyro` | `printh 23 01 42` | `B` robot self-stabilize on/off |
| `hHeight` | `printh 23 02 48`<br>`prints hHeight.val,1` | `H` + height byte (Touch Move + Release) | | `hSpeed` | `printh 23 02 56` + `prints hSpeed.val,1` (move + release) | `V` + speed 0..255 |
| `bZero` | `printh 23 01 5A` | `Z` zero handset level | | `hHeight` | `printh 23 02 48` + `prints hHeight.val,1` (move + release) | `H` + height 0..200, 100 = default |
| `bCal` | `printh 23 01 43` | `C` start / save stick calibration | | `bCal` | `printh 23 01 43` | `C` start / save stick calibration |
| `bCalX` | `printh 23 01 51` | `Q` cancel calibration | | `bCalX` | `printh 23 01 51` | `Q` cancel stick calibration |
| `bZero` | `printh 23 01 5A` | `Z` zero handset level |
| `bUpd` | `printh 23 01 55` | `U` display update mode (second tap within 3 s) |
| `bCalOn` | `printh 23 01 4F` | `O` servo calibration start/stop |
| `bL0`–`bL5` | `printh 23 02 4C 0n` | `L` + leg (0 FR, 1 MR, 2 RR, 3 RL, 4 ML, 5 FL) |
| `bJ0`–`bJ2` | `printh 23 02 4A 0n` | `J` + joint (0 coxa, 1 femur, 2 tibia) |
| `bM10` `bM1` `bP1` `bP10` `bZeroT` | `printh 23 02 54 0n` | `T` + trim step: 0 −10, 1 −1, 2 +1, 3 +10, 4 zero (µs) |
| `bSave` | `printh 23 01 57` | `W` save trims on the robot |
| `bRevert` | `printh 23 01 45` | `E` reload trims from the robot's EEPROM |
**Uncheck "Send Component ID"** on every component on `control`: those To add a control: pick an unused command byte, add it to
standard touch frames only add serial traffic. The one exception is the old `NextionUI::decodeCustom()` and `UiCmd`, handle it in `RemoteState::handle()`,
page 1 `b2` upload button (see the top). add the component in `make_remote_hmi.py`, and a row here.
Protocol details and the parser live in `NextionUI.h/.cpp`. To add a ## What the remote writes (remote → display)
control, pick an unused command byte, add it to `NextionUI::decodeCustom()`
and `UiCmd`, handle it in `RemoteState::handle()`, and add a row here. | Page | Components |
|------|-----------|
| all but splash/upload | `tLink` txt/pco, `tState` txt/pco |
| drive | `bArm` `bStand` txt/bco, `bGait` `bMode` `bRs` txt, `bRGyro` `bBGyro` bco, `hSpeed` `hHeight` val, `tSpeed` `tHeight` `tMsg` txt |
| status | `tRRoll` `tRPitch` `tBRoll` `tBPitch` txt, `hRRoll` `hRPitch` `hBRoll` `hBPitch` val (0..90 = ±45°), `hLX` `hLY` `hLT` `hRX` `hRY` `hRT` val (0..200 = ±1000), `tBatt` `tRadio` txt. The indicator sliders have touch switched off (`tsw` in Preinitialize) |
| servos | `tJ0`–`tJ17` txt (index = leg × 3 + joint), `jJ0`–`jJ17` val (position in the servo's travel), `tSrv` txt/pco |
| cal | `bCalOn` txt/bco, `bL0`–`bL5` `bJ0`–`bJ2` bco (selected = green), `tTrim` txt, `bSave` bco (orange = unsaved), `tMsg` txt |
| setup | `bCal` txt/bco, `bUpd` txt/bco, `tVer` txt, `tMsg` txt |
`txt_maxl` of each text is set in the generator to fit the longest string
the firmware writes. Keep them in step when you change a format string.
+208 -63
View File
@@ -6,27 +6,29 @@ static constexpr uint16_t COL_ON = 2016; // green
static constexpr uint16_t COL_OFF = 50712; // Nextion default light gray static constexpr uint16_t COL_OFF = 50712; // Nextion default light gray
static constexpr uint16_t COL_DANGER = 63488; // red static constexpr uint16_t COL_DANGER = 63488; // red
static constexpr uint16_t COL_WARN = 64512; // orange static constexpr uint16_t COL_WARN = 64512; // orange
static constexpr uint16_t COL_TEXT = 65535; // white (top bar text)
static constexpr uint16_t COL_DIM = 33808; // gray (no data)
// The default 64-byte TX buffer would make a full redraw (~400 bytes at // The default 64-byte TX buffer would make a full redraw (~1 kB at 115200
// 115200 baud) block the TX loop for ~35 ms. Give it room to queue. // baud) block the TX loop for ~90 ms. Give it room to queue.
static uint8_t tx_extra[1024]; static uint8_t tx_extra[2048];
static uint8_t rx_extra[128]; static uint8_t rx_extra[128];
// Every control lives on NEXTION_CONTROL_PAGE with vscope = global, so it's static const char* const LEG_NAMES[6] = { "FR", "MR", "RR", "RL", "ML", "FL" }; // robot leg order
// addressed as page.component and can be updated whichever page is showing. static const char* const JOINT_NAMES[3] = { "coxa", "femur", "tibia" };
#define P NEXTION_CONTROL_PAGE "."
void NextionUI::begin() { void NextionUI::begin() {
NEXTION_SERIAL.begin(NEXTION_BAUD); NEXTION_SERIAL.begin(NEXTION_BAUD);
NEXTION_SERIAL.addMemoryForWrite(tx_extra, sizeof(tx_extra)); NEXTION_SERIAL.addMemoryForWrite(tx_extra, sizeof(tx_extra));
NEXTION_SERIAL.addMemoryForRead(rx_extra, sizeof(rx_extra)); NEXTION_SERIAL.addMemoryForRead(rx_extra, sizeof(rx_extra));
cmd("bkcmd=0"); // no success/fail replies - we never need them (and a cmd("bkcmd=0"); // no success/fail replies - we never need them (and a write
// write to a page the HMI doesn't have is then just ignored) // to a component that isn't on the showing page is ignored)
cmd("sendme"); // which page is showing? (the display may have kept running)
force_full_ = true; force_full_ = true;
} }
void NextionUI::cmd(const char* fmt, ...) { void NextionUI::cmd(const char* fmt, ...) {
char buf[80]; char buf[96];
va_list ap; va_list ap;
va_start(ap, fmt); va_start(ap, fmt);
vsnprintf(buf, sizeof(buf), fmt, ap); vsnprintf(buf, sizeof(buf), fmt, ap);
@@ -37,6 +39,30 @@ void NextionUI::cmd(const char* fmt, ...) {
NEXTION_SERIAL.write(0xFF); NEXTION_SERIAL.write(0xFF);
} }
void NextionUI::put(const char* fmt, ...) {
char buf[96];
va_list ap;
va_start(ap, fmt);
vsnprintf(buf, sizeof(buf), fmt, ap);
va_end(ap);
uint32_t h = 2166136261u; // FNV-1a
for (const char* c = buf; *c; c++) h = (h ^ (uint8_t)*c) * 16777619u;
h |= 1; // 0 = empty slot
bool cached = slot_ < MAX_SLOTS;
if (cached && !full_ && cache_[slot_] == h) { slot_++; return; }
if (cached) cache_[slot_] = h;
slot_++;
NEXTION_SERIAL.print(buf);
NEXTION_SERIAL.write(0xFF);
NEXTION_SERIAL.write(0xFF);
NEXTION_SERIAL.write(0xFF);
}
void NextionUI::setPage(uint8_t page) {
page_ = page;
force_full_ = true; // a newly shown page has its HMI defaults: write everything
}
// ---- Receive ---- // ---- Receive ----
bool NextionUI::decodeCustom(UiEvent& out) { bool NextionUI::decodeCustom(UiEvent& out) {
@@ -55,26 +81,37 @@ bool NextionUI::decodeCustom(UiEvent& out) {
case 'Z': out.cmd = UiCmd::ZeroGyro; break; case 'Z': out.cmd = UiCmd::ZeroGyro; break;
case 'C': out.cmd = UiCmd::CalStartSave; break; case 'C': out.cmd = UiCmd::CalStartSave; break;
case 'Q': out.cmd = UiCmd::CalCancel; break; case 'Q': out.cmd = UiCmd::CalCancel; break;
case 'U': out.cmd = UiCmd::NextionUpload; break; case 'U': out.cmd = UiCmd::UploadRequest; break;
case 'O': out.cmd = UiCmd::ServoCalToggle; break;
case 'L': out.cmd = UiCmd::ServoCalLeg; break;
case 'J': out.cmd = UiCmd::ServoCalJoint; break;
case 'T': out.cmd = UiCmd::ServoCalTrim; break;
case 'W': out.cmd = UiCmd::ServoCalSave; break;
case 'E': out.cmd = UiCmd::ServoCalRevert; break;
case 'P':
if (out.value == page_) return false; // same page again: nothing changes
setPage(out.value);
out.cmd = UiCmd::PageShown;
break;
default: return false; default: return false;
} }
return true; return true;
} }
// Standard Nextion frames (terminated FF FF FF). Two matter: // Standard Nextion frames (terminated FF FF FF). Two matter:
// 0x88 / 0x00 display (re)started -> redraw everything // 0x88 / 0x00 display (re)started -> redraw everything (its start page
// 0x65 page id event touch event from a component with "Send Component // then reports itself with 'P')
// ID" ticked - how the old HMI's upload button (b2) // 0x66 page reply to `sendme`: the page that is showing
// reports, so it keeps working unchanged.
bool NextionUI::decodeNative(UiEvent& out) { bool NextionUI::decodeNative(UiEvent& out) {
if (native_buf_[0] == 0x88 || native_buf_[0] == 0x00) { if (native_buf_[0] == 0x88 || native_buf_[0] == 0x00) {
force_full_ = true; force_full_ = true;
return false; return false;
} }
if (native_buf_[0] == 0x65 && native_len_ >= 4 && if (native_buf_[0] == 0x66 && native_len_ >= 5) {
native_buf_[1] == NEXTION_UPLOAD_BUTTON_PAGE && native_buf_[2] == NEXTION_UPLOAD_BUTTON_ID) { if (native_buf_[1] == page_) return false;
out.cmd = UiCmd::NextionUpload; setPage(native_buf_[1]);
out.value = 0; out.cmd = UiCmd::PageShown;
out.value = native_buf_[1];
return true; return true;
} }
return false; return false;
@@ -131,62 +168,170 @@ bool NextionUI::poll(UiEvent& out) {
// ---- Render ---- // ---- Render ----
void NextionUI::pushState(const UiView& v, bool full) { static const char* robotStateName(uint8_t s) {
const UiView& s = shown_; switch (s) {
if (full || v.armed != s.armed) { case TSTATE_DISARMED: return "DISARMED";
cmd(P "bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED"); case TSTATE_SIT: return "SIT";
cmd(P "bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF); case TSTATE_STAND: return "STAND";
case TSTATE_WALKING: return "WALKING";
case TSTATE_SERVO_CAL: return "SERVO CAL";
default: return "?";
} }
if (full || v.stand != s.stand) {
cmd(P "bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
cmd(P "bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
}
if (full || v.gait_wave != s.gait_wave) cmd(P "bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
if (full || v.mode_b != s.mode_b) cmd(P "bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
if (full || v.rstick_xlate != s.rstick_xlate) cmd(P "bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
if (full || v.remote_gyro != s.remote_gyro) cmd(P "bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
if (full || v.robot_gyro != s.robot_gyro) cmd(P "bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
if (full || v.speed_target != s.speed_target) {
cmd(P "hSpeed.val=%u", v.speed_target);
cmd(P "nSpeed.val=%u", (v.speed_target * 100 + 127) / 255);
}
if (full || v.height != s.height) {
cmd(P "hHeight.val=%d", v.height / 10 + 100); // slider 0..200, 100 = default
cmd(P "nHeight.val=%d", v.height / 10); // -100..100 %
}
if (full || v.calibrating != s.calibrating) {
cmd(P "bCal.txt=\"%s\"", v.calibrating ? "SAVE CAL" : "CAL");
cmd(P "bCal.bco=%u", v.calibrating ? COL_WARN : COL_OFF);
}
if (full || v.message != s.message) cmd(P "tMsg.txt=\"%s\"", v.message);
} }
void NextionUI::pushLink(const UiView& v) { // "7.2V", or "--" when the battery isn't measured.
if (v.link_ok) cmd(P "tLink.txt=\"LINK %u%%\"", v.link_pct); static const char* volts(char* buf, size_t n, uint16_t mv) {
else cmd(P "tLink.txt=\"NO LINK\""); if (mv == 0) snprintf(buf, n, "--");
cmd(P "tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER); else snprintf(buf, n, "%u.%uV", mv / 1000, (mv % 1000) / 100);
// The old HMI's own status texts, same wording as the old firmware. return buf;
for (const char* t : NEXTION_STATUS_TEXTS) }
cmd("%s.txt=\"%s\"", t, v.link_ok ? "connected" : "not connected");
// Angles are shown to 0.5 deg, so sensor noise doesn't repaint every frame.
static float halfDeg(float d) { return roundf(d * 2.0f) / 2.0f; }
void NextionUI::renderTopBar(const UiView& v) {
if (v.link_ok) put("tLink.txt=\"LINK %u%%\"", v.link_pct);
else put("tLink.txt=\"NO LINK\"");
put("tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER);
char b[8];
if (v.tele_ok) put("tState.txt=\"ROBOT %s %s\"", robotStateName(v.tele.state), volts(b, sizeof(b), v.tele.battery_mv));
else put("tState.txt=\"ROBOT: NO DATA\"");
put("tState.pco=%u", v.tele_ok ? COL_TEXT : COL_DIM);
}
void NextionUI::renderDrive(const UiView& v) {
put("bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED");
put("bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF);
put("bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
put("bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
put("bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
put("bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
put("bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
put("bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
put("bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
put("hSpeed.val=%u", v.speed_target);
put("tSpeed.txt=\"%u%%\"", (v.speed_target * 100 + 127) / 255);
put("hHeight.val=%d", v.height / 10 + 100); // slider 0..200, 100 = default
put("tHeight.txt=\"%+d%%\"", v.height / 10);
put("tMsg.txt=\"%s\"", v.message);
}
void NextionUI::renderStatus(const UiView& v) {
// Handset IMU, sliders 0..90 = -45..+45 deg
if (v.imu_ok) {
put("tRRoll.txt=\"roll %+.1f\"", halfDeg(v.imu_roll));
put("tRPitch.txt=\"pitch %+.1f\"", halfDeg(v.imu_pitch));
} else {
put("tRRoll.txt=\"roll no IMU\"");
put("tRPitch.txt=\"pitch no IMU\"");
}
put("hRRoll.val=%d", (int)constrain(lroundf(v.imu_roll) + 45, 0L, 90L));
put("hRPitch.val=%d", (int)constrain(lroundf(v.imu_pitch) + 45, 0L, 90L));
// Robot IMU, from telemetry
float br = v.tele.roll_ddeg / 10.0f, bp = v.tele.pitch_ddeg / 10.0f;
bool robot_imu = v.tele_ok && (v.tele.status & TSTAT_IMU_OK);
if (robot_imu) {
put("tBRoll.txt=\"roll %+.1f\"", halfDeg(br));
put("tBPitch.txt=\"pitch %+.1f\"", halfDeg(bp));
} else {
put("tBRoll.txt=\"roll %s\"", v.tele_ok ? "no IMU" : "--");
put("tBPitch.txt=\"pitch %s\"", v.tele_ok ? "no IMU" : "--");
}
put("hBRoll.val=%d", robot_imu ? (int)constrain(lroundf(br) + 45, 0L, 90L) : 45);
put("hBPitch.val=%d", robot_imu ? (int)constrain(lroundf(bp) + 45, 0L, 90L) : 45);
// Sticks, sliders 0..200 = -1000..+1000
static const char* const STICKS[NUM_AXES] = { "hLX", "hLY", "hLT", "hRX", "hRY", "hRT" };
for (int i = 0; i < NUM_AXES; i++) put("%s.val=%d", STICKS[i], (v.sticks[i] + 1000) / 10);
char a[8], b[8];
put("tBatt.txt=\"battery remote %s robot %s\"", volts(a, sizeof(a), v.battery_mv),
v.tele_ok ? volts(b, sizeof(b), v.tele.battery_mv) : "--");
if (!v.link_ok) put("tRadio.txt=\"radio no link\"");
else if (v.tele_ok) put("tRadio.txt=\"radio %u%% acked, robot hears %u/s\"", v.link_pct, v.tele.rx_per_sec);
else put("tRadio.txt=\"radio %u%% acked, no telemetry\"", v.link_pct);
}
void NextionUI::renderServos(const UiView& v) {
for (int i = 0; i < 18; i++) {
int joint = i % 3;
if (v.tele_ok) {
int deg = v.tele.joint_deg[i] - (joint == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0);
// Bar = position within the servo's travel: coxa/femur -90..90, tibia -180..0.
int pct = (deg + (joint == 2 ? 180 : 90)) * 100 / 180;
put("tJ%d.txt=\"%-5s %+4d\"", i, JOINT_NAMES[joint], deg);
put("jJ%d.val=%d", i, constrain(pct, 0, 100));
} else {
put("tJ%d.txt=\"%-5s --\"", i, JOINT_NAMES[joint]);
put("jJ%d.val=0", i);
}
}
const char* s = !v.tele_ok ? "NO DATA"
: v.tele.state == TSTATE_SERVO_CAL ? "CENTRE"
: (v.tele.status & TSTAT_SERVOS_ON) ? "LIVE"
: "RELAXED";
put("tSrv.txt=\"%s\"", s);
put("tSrv.pco=%u", !v.tele_ok ? COL_DIM : (v.tele.status & TSTAT_SERVOS_ON) ? COL_ON : 0);
}
void NextionUI::renderCal(const UiView& v) {
put("bCalOn.txt=\"%s\"", v.servo_cal ? "STOP" : "START");
put("bCalOn.bco=%u", v.servo_cal ? COL_WARN : COL_OFF);
for (int i = 0; i < 6; i++) put("bL%d.bco=%u", i, v.servo_sel / 3 == i ? COL_ON : COL_OFF);
for (int j = 0; j < 3; j++) put("bJ%d.bco=%u", j, v.servo_sel % 3 == j ? COL_ON : COL_OFF);
const char* leg = LEG_NAMES[v.servo_sel / 3];
const char* joint = JOINT_NAMES[v.servo_sel % 3];
if (!v.servo_cal) put("tTrim.txt=\"%s %s\"", leg, joint);
else if (!v.trim_synced) put("tTrim.txt=\"%s %s ...\"", leg, joint);
else put("tTrim.txt=\"%s %s %+dus\"", leg, joint, v.trim_us);
bool dirty = v.tele_ok && (v.tele.status & TSTAT_CAL_DIRTY);
put("bSave.bco=%u", dirty ? COL_WARN : COL_OFF);
const char* hint = v.message[0] ? v.message
: !v.servo_cal ? "Robot on a stand, disarmed: START"
: !v.tele_ok ? "No telemetry from the robot"
: v.tele.state != TSTATE_SERVO_CAL ? "Waiting for the robot"
: dirty ? "Unsaved trims: SAVE keeps them"
: "Trim until the horn is square";
put("tMsg.txt=\"%s\"", hint);
}
void NextionUI::renderSetup(const UiView& v) {
put("bCal.txt=\"%s\"", v.stick_calibrating ? "SAVE CAL" : "CAL STICKS");
put("bCal.bco=%u", v.stick_calibrating ? COL_WARN : COL_OFF);
put("bUpd.txt=\"%s\"", v.upload_confirm ? "TAP AGAIN" : "UPDATE DISPLAY");
put("bUpd.bco=%u", v.upload_confirm ? COL_WARN : COL_OFF);
put("tVer.txt=\"remote fw %s protocol v%u radio ch %u\"", __DATE__, HXP_PROTOCOL_VERSION, HXP_RADIO_CHANNEL);
put("tMsg.txt=\"%s\"", v.message);
} }
void NextionUI::render(const UiView& v, uint32_t now_ms) { void NextionUI::render(const UiView& v, uint32_t now_ms) {
bool full = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS; uint32_t interval = page_ == PAGE_SERVOS ? NEXTION_SERVOS_RENDER_MS : NEXTION_RENDER_MS;
if (full) { if (!force_full_ && (now_ms - last_render_ms_) < interval) return;
// Never block the TX loop on the display: wait for the buffer to drain.
if (NEXTION_SERIAL.availableForWrite() < NEXTION_TX_MIN_FREE) return;
last_render_ms_ = now_ms;
full_ = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS;
if (full_) {
force_full_ = false; force_full_ = false;
last_full_ms_ = now_ms; last_full_ms_ = now_ms;
if (page_ == PAGE_UNKNOWN) cmd("sendme"); // still don't know the page: ask again
} }
slot_ = 0;
pushState(v, full); switch (page_) {
case PAGE_DRIVE: renderTopBar(v); renderDrive(v); break;
// Link quality changes nearly every packet; repaint it at a readable rate, case PAGE_STATUS: renderTopBar(v); renderStatus(v); break;
// but immediately when the link comes or goes. case PAGE_SERVOS: renderTopBar(v); renderServos(v); break;
if (full || v.link_ok != shown_.link_ok || (now_ms - last_link_ms_) >= NEXTION_LINK_REFRESH_MS) { case PAGE_CAL: renderTopBar(v); renderCal(v); break;
pushLink(v); case PAGE_SETUP: renderTopBar(v); renderSetup(v); break;
last_link_ms_ = now_ms; default: break; // splash / upload / unknown: nothing to show
} }
shown_ = v;
} }
// ---- Upload passthrough ---- // ---- Upload passthrough ----
+31 -12
View File
@@ -7,17 +7,21 @@
// Nextion -> remote: custom frames the HMI's touch events send with printh: // Nextion -> remote: custom frames the HMI's touch events send with printh:
// '#' <len> <cmd> [value] e.g. printh 23 01 41 (ToggleArm) // '#' <len> <cmd> [value] e.g. printh 23 01 41 (ToggleArm)
// printh 23 02 56 + prints hSpeed.val,1 // printh 23 02 56 + prints hSpeed.val,1
// Command bytes are listed in NEXTION.md and in NextionUI.cpp. // Command bytes are listed in NEXTION.md and in NextionUI.cpp. Every
// Native Nextion return frames (terminated FF FF FF) are also parsed: a // page's Preinitialize event sends 'P' + its page id, so the remote always
// display reset (0x88 "ready") triggers a full redraw, and the old HMI's // knows which page is showing. Native Nextion return frames (terminated
// upload button sends a standard touch event (0x65) that's recognized. // FF FF FF) are parsed too: a display reset (0x88 "ready") triggers a
// full redraw, and the reply to `sendme` (0x66 page) gives the page after
// the remote itself restarted.
// //
// remote -> Nextion: plain instructions ("bArm.txt=\"ARMED\"" + FF FF FF). // remote -> Nextion: plain instructions ("bArm.txt=\"ARMED\"" + FF FF FF).
// //
// The remote is authoritative: buttons are plain buttons (not dual-state), // Only the page that is showing is written (by local component names), and
// they only send requests, and render() repaints whatever the remote state // only what changed: every instruction is hashed into a per-page cache. The
// actually is. A Nextion reboot or a dropped byte self-heals on the next // remote is authoritative: buttons are plain buttons, they only send
// periodic full refresh. // requests, and render() repaints whatever the remote state actually is. A
// Nextion reboot or a dropped byte self-heals on the next periodic full
// refresh.
class NextionUI { class NextionUI {
public: public:
void begin(); void begin();
@@ -35,14 +39,29 @@ private:
uint8_t native_buf_[8]; uint8_t native_buf_[8];
uint8_t native_len_ = 0, ff_run_ = 0; uint8_t native_len_ = 0, ff_run_ = 0;
UiView shown_; uint8_t page_ = PAGE_UNKNOWN;
bool force_full_ = true; bool force_full_ = true;
uint32_t last_full_ms_ = 0; uint32_t last_full_ms_ = 0;
uint32_t last_link_ms_ = 0; uint32_t last_render_ms_ = 0;
// Change cache: one hash per instruction, in the order a page's render
// issues them (the same every time).
static constexpr int MAX_SLOTS = 96;
uint32_t cache_[MAX_SLOTS] = {};
uint8_t slot_ = 0;
bool full_ = false;
void cmd(const char* fmt, ...); void cmd(const char* fmt, ...);
void pushState(const UiView& v, bool full); void put(const char* fmt, ...); // like cmd(), but only if it changed
void pushLink(const UiView& v); void setPage(uint8_t page);
void renderTopBar(const UiView& v);
void renderDrive(const UiView& v);
void renderStatus(const UiView& v);
void renderServos(const UiView& v);
void renderCal(const UiView& v);
void renderSetup(const UiView& v);
bool decodeCustom(UiEvent& out); bool decodeCustom(UiEvent& out);
bool decodeNative(UiEvent& out); bool decodeNative(UiEvent& out);
}; };
+3
View File
@@ -14,6 +14,9 @@ CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const
p.seq = seq_++; p.seq = seq_++;
p.speed_scale = st.speedCurrent(); p.speed_scale = st.speedCurrent();
p.body_height = st.height(); // a setting, not motion: sent even while disarmed p.body_height = st.height(); // a setting, not motion: sent even while disarmed
p.cal_cmd = st.servoCalCmd(now_ms); // CAL_NONE unless the CAL page is calibrating (only while disarmed)
p.cal_servo = st.servoSel();
p.cal_value = st.trimTarget();
uint8_t f = 0; uint8_t f = 0;
if (st.armed()) f |= FLAG_ARMED; if (st.armed()) f |= FLAG_ARMED;
+56 -14
View File
@@ -8,11 +8,13 @@ at 100 Hz.
## Planned ## Planned
- **Battery level** (not built yet): read the 18650's voltage through a - **Battery level** (divider not built yet): read the 18650's voltage through a
~100k/100k divider from the cell's + (not the boost board's 5 V output) ~100k/100k divider from the cell's + (not the boost board's 5 V output)
into A6 (pin 20, free), with 100 nF to GND, and show the charge on the into A6 (pin 20, free), with 100 nF to GND. Details in
Nextion with a low-battery warning. Details in `../hardware/openscad/PLANNING.md` (notes for future designs). The
`../hardware/openscad/PLANNING.md` (notes for future designs). firmware side is done (`Battery`, shown on the STATUS page): set
`BATTERY_SENSE_PIN = A6` in `Config.h` once it's wired. A low-battery
warning is still to do.
- **IMU on the normal pin headers** (suggestion, not done): with the GY-521 on - **IMU on the normal pin headers** (suggestion, not done): with the GY-521 on
the main board, wire it to Wire (SDA 18 / SCL 19) instead of Wire2 (the the main board, wire it to Wire (SDA 18 / SCL 19) instead of Wire2 (the
@@ -37,7 +39,7 @@ From the `2026-current` folder: `pio run -e remote`, flash with
| nRF24L01 | CE / CSN | 9 / 10 | | nRF24L01 | CE / CSN | 9 / 10 |
| nRF24L01 | VCC | 3.3 V, with a 10–100 µF cap right at the module | | nRF24L01 | VCC | 3.3 V, with a 10–100 µF cap right at the module |
| MPU6050 | SDA / SCL | **25 / 24 (Wire2)**, solder pads on the underside of the 4.0: not 18/19, those are stick pins A4/A5 | | MPU6050 | SDA / SCL | **25 / 24 (Wire2)**, solder pads on the underside of the 4.0: not 18/19, those are stick pins A4/A5 |
| Nextion | TX → RX1, RX ← TX1 | 0 / 1 (Serial1), 5 V supply | | Nextion | TX → RX2, RX ← TX2 | 7 / 8 (Serial2), 5 V supply (found on the real remote) |
| Sticks | 6 axes | A0–A5. Which axis is on which pin isn't known yet: reorder `STICK_PINS` until the `p` dump reads right | | Sticks | 6 axes | A0–A5. Which axis is on which pin isn't known yet: reorder `STICK_PINS` until the `p` dump reads right |
**MPU6050 rewiring:** the old remote had its MPU code switched off, probably **MPU6050 rewiring:** the old remote had its MPU code switched off, probably
@@ -64,14 +66,44 @@ steers: forward+left or back+left walk while turning left, and left alone
turns on the spot for as long as you hold it. Let go and the robot holds turns on the spot for as long as you hold it. Let go and the robot holds
its heading. its heading.
Touchscreen: ARM / DISARM, SIT/STAND, TRIPOD/WAVE, DIRECT/HEADING, Touchscreen (`NEXTION.md` has the details), with tabs along the bottom and
R: ROTATE/MOVE, REMOTE GYRO, ROBOT GYRO, speed slider, LEVEL (re-zero the a red DISARM on every page:
handset tilt), CAL / CANCEL CAL. `NEXTION.md` has the HMI build guide.
- **DRIVE:** ARM / DISARM, SIT/STAND, TRIPOD/WAVE, DIRECT/HEADING,
R: ROTATE/MOVE, REMOTE GYRO, ROBOT GYRO, speed and height sliders.
- **STATUS:** handset and robot IMU roll/pitch, the six stick axes, both
batteries, radio quality and the robot's received packet rate.
- **SERVOS:** all 18 joint angles the robot is commanding, as a top view.
- **CAL:** servo centre calibration (see below).
- **SETUP:** stick calibration (CAL STICKS / CANCEL), LEVEL REMOTE (re-zero
the handset tilt), UPDATE DISPLAY, backlight.
Everything about the robot comes from its **telemetry**, which rides back on
the radio's ack payloads (`Telemetry` in `HexapodProtocol.h`): `RadioLink`
reads it after every send, and it counts as stale after
`TELEMETRY_STALE_MS`.
## Servo centre calibration (CAL page)
1. Put the robot on a stand, legs free, and disarm.
2. Tap **START**. The robot drives every servo to its centre pose: coxa 0,
femur 0, tibia −90 (legs straight out, shins straight down).
3. Pick a leg (FR MR RR RL ML FL) and a joint. The remote first reads that
servo's trim back from the robot (the trim box shows `...` until then).
4. Step the trim with −10 / −1 / +1 / +10 µs (or ZERO) until the horn is
exactly square. SAVE TO ROBOT turns orange while there are unsaved
trims.
5. **SAVE TO ROBOT** stores all 18 trims in the robot's EEPROM. **REVERT**
drops unsaved changes. **STOP**, leaving the page, or a link loss relaxes
the servos. Arming is refused while calibrating.
### Safety interlocks (in `RemoteState`) ### Safety interlocks (in `RemoteState`)
- **Arming** is refused unless every stick axis is centered, and never - **Arming** is refused unless every stick axis is centered, and never
allowed during calibration. allowed during stick or servo calibration. Servo calibration only starts
while disarmed, and stops when you leave the CAL page or the link drops.
- **UPDATE DISPLAY** needs a second tap within 3 s and is refused while
armed.
- **Disarming** always clears the stand request. The robot arms into SIT, - **Disarming** always clears the stand request. The robot arms into SIT,
so re-arming never stands it up by itself. so re-arming never stands it up by itself.
- **Link loss:** if no ack arrives for `LINK_LOST_MS` (300 ms, matching the - **Link loss:** if no ack arrives for `LINK_LOST_MS` (300 ms, matching the
@@ -122,6 +154,12 @@ display attached.
| `c` | start / save calibration | `x` | cancel calibration | | `c` | start / save calibration | `x` | cancel calibration |
| `p` | print status (raw + mapped sticks, IMU, link, state, cal) | `d` | toggle 10 Hz packet dump | | `p` | print status (raw + mapped sticks, IMU, link, state, cal) | `d` | toggle 10 Hz packet dump |
| `R` | reset stick cal to Config defaults | `U` | Nextion upload mode (USB ↔ display; power-cycle to leave) | | `R` | reset stick cal to Config defaults | `U` | Nextion upload mode (USB ↔ display; power-cycle to leave) |
| `o` | servo calibration start/stop | `n` | next servo (leg × 3 + joint) |
| `,` / `.` | trim −1 / +1 µs | `<` / `>` | trim −10 / +10 µs |
| `0` | trim to zero | `w` / `e` | save trims on the robot / revert |
`p` also prints the robot's telemetry (state, IMU, battery, joint angles)
and the servo-calibration state.
## Bench-check checklist ## Bench-check checklist
@@ -131,10 +169,14 @@ display attached.
2. **IMU mounting:** with `p`, dip the right edge down to get +roll, and 2. **IMU mounting:** with `p`, dip the right edge down to get +roll, and
tip the far edge down to get +pitch. If not, fix `IMU_AXIS_FROM_CHIP` / tip the far edge down to get +pitch. If not, fix `IMU_AXIS_FROM_CHIP` /
`IMU_AXIS_SIGN`. `IMU_AXIS_SIGN`.
3. **Nextion baud:** `bauds=115200` must be in Program.s, or the display 3. **Nextion baud:** the generated HMI sets `bauds=115200` in Program.s.
shows nothing useful. The display still runs the old HMI (probably 9600) until you upload
4. **Radio:** with the robot powered, `tLink` / `p` should show ~100%. If the new one; Nextion Editor finds the baud rate by itself. For that
it reads NO LINK, check that both boards were built from the same first upload use serial key `U`: the old HMI's UPLOAD button isn't
`HexapodProtocol.h`. The static payload size is part of the handshake. recognised any more.
4. **Radio:** with the robot powered, `tLink` / `p` should show ~100%, and
`p` should print robot telemetry. If it reads NO LINK, check that both
boards were built from the same `HexapodProtocol.h`: each end drops
packets of the wrong size.
5. **End to end with the robot on a stand:** use `d` to dump packets and 5. **End to end with the robot on a stand:** use `d` to dump packets and
confirm each field changes the way the table above says. confirm each field changes the way the table above says.
+16 -1
View File
@@ -11,7 +11,10 @@ static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
bool RadioLink::begin() { bool RadioLink::begin() {
present_ = radio.begin(); present_ = radio.begin();
if (present_) { if (present_) {
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // must match the robot exactly (static payloads) // The robot answers with Telemetry in its ack payloads, which needs
// dynamic payload lengths on both radios. Each end checks the length of
// what it receives against its own struct instead (see HexapodProtocol.h).
radio.enableAckPayload();
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL); radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE); radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
radio.setChannel(HXP_RADIO_CHANNEL); radio.setChannel(HXP_RADIO_CHANNEL);
@@ -30,6 +33,18 @@ bool RadioLink::send(const CmdPacket& p) {
last_ack_ms_ = millis(); last_ack_ms_ = millis();
ever_acked_ = true; ever_acked_ = true;
} }
// An ack payload lands in the RX FIFO. Drain it every time, so it can't fill up.
while (radio.available()) {
uint8_t len = radio.getDynamicPayloadSize();
if (len == sizeof(Telemetry)) {
radio.read(&telemetry_, sizeof(telemetry_));
has_telemetry_ = true;
telemetry_ms_ = millis();
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
uint8_t junk[32]; // another protocol version: ignore it
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
}
}
return acked; return acked;
} }
+11
View File
@@ -1,8 +1,10 @@
#pragma once #pragma once
#include <HexapodProtocol.h> #include <HexapodProtocol.h>
#include "Config.h"
// nRF24L01 transmitter. Uses hardware auto-ack to measure the link: the // nRF24L01 transmitter. Uses hardware auto-ack to measure the link: the
// robot's radio acks every packet it receives, so an ack = the robot heard us. // robot's radio acks every packet it receives, so an ack = the robot heard us.
// The robot's Telemetry comes back as the ack's payload.
class RadioLink { class RadioLink {
public: public:
bool begin(); // false if the radio chip didn't respond bool begin(); // false if the radio chip didn't respond
@@ -13,10 +15,19 @@ public:
bool linkOk() const { return link_ok_; } bool linkOk() const { return link_ok_; }
uint8_t qualityPct() const; // acked share of the last 32 packets uint8_t qualityPct() const; // acked share of the last 32 packets
// Latest telemetry from the robot, and whether it's recent enough to show.
const Telemetry& telemetry() const { return telemetry_; }
bool telemetryFresh(uint32_t now_ms) const {
return has_telemetry_ && (now_ms - telemetry_ms_) < TELEMETRY_STALE_MS;
}
private: private:
bool present_ = false; bool present_ = false;
bool link_ok_ = false; bool link_ok_ = false;
uint32_t last_ack_ms_ = 0; uint32_t last_ack_ms_ = 0;
bool ever_acked_ = false; bool ever_acked_ = false;
uint32_t history_ = 0; // 1 bit per packet, newest in bit 0 uint32_t history_ = 0; // 1 bit per packet, newest in bit 0
Telemetry telemetry_;
bool has_telemetry_ = false;
uint32_t telemetry_ms_ = 0;
}; };
+111 -6
View File
@@ -17,6 +17,26 @@ void RemoteState::disarm() {
stand_ = false; stand_ = false;
} }
void RemoteState::stopServoCal() {
servo_cal_ = false;
trim_synced_ = false;
cal_op_ = CAL_NONE;
}
// A new selection is only trimmed once the robot has reported that servo's
// current trim (CAL_HOLD until then), so a stale value is never sent.
void RemoteState::selectServo(uint8_t servo) {
if (servo == servo_sel_) return;
servo_sel_ = servo;
trim_synced_ = false;
}
uint8_t RemoteState::servoCalCmd(uint32_t now_ms) const {
if (!servo_cal_ || armed_) return CAL_NONE;
if (cal_op_ != CAL_NONE && now_ms < cal_op_until_ms_) return cal_op_;
return trim_synced_ ? CAL_SET : CAL_HOLD;
}
bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) { bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) {
const StickFrame& sticks = stick_hw.frame(); const StickFrame& sticks = stick_hw.frame();
switch (e.cmd) { switch (e.cmd) {
@@ -30,6 +50,7 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
case UiCmd::ToggleArm: case UiCmd::ToggleArm:
if (armed_) { disarm(); return true; } if (armed_) { disarm(); return true; }
if (calibrating_) { setMessage("Finish calibration first", now); return false; } if (calibrating_) { setMessage("Finish calibration first", now); return false; }
if (servo_cal_) { setMessage("Stop servo calibration first", now); return false; }
if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; } if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; }
armed_ = true; armed_ = true;
return true; return true;
@@ -81,6 +102,66 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
case UiCmd::NextionUpload: case UiCmd::NextionUpload:
return false; // not operator state - remote.ino handles it return false; // not operator state - remote.ino handles it
case UiCmd::UploadRequest:
if (armed_) { setMessage("Disarm first", now); return false; }
if (now < upload_confirm_until_ms_) { upload_go_ = true; return true; }
upload_confirm_until_ms_ = now + UPLOAD_CONFIRM_MS;
setMessage("Tap again to enter display update mode", now);
return true;
case UiCmd::PageShown:
page_ = e.value;
// Servo calibration only runs while its page is showing.
if (servo_cal_ && page_ != PAGE_CAL) stopServoCal();
upload_confirm_until_ms_ = 0;
return true;
case UiCmd::ServoCalToggle:
if (servo_cal_) { stopServoCal(); setMessage("Servos relaxed", now); return true; }
if (armed_) { setMessage("Disarm to calibrate servos", now); return false; }
servo_cal_ = true;
trim_synced_ = false;
setMessage("Robot holds every servo at centre", now);
return true;
case UiCmd::ServoCalLeg:
if (e.value >= 6) return false;
selectServo(e.value * 3 + servo_sel_ % 3);
return true;
case UiCmd::ServoCalJoint:
if (e.value >= 3) return false;
selectServo(servo_sel_ / 3 * 3 + e.value);
return true;
case UiCmd::ServoCalNext:
selectServo((servo_sel_ + 1) % 18);
return true;
case UiCmd::ServoCalTrim: {
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
if (!trim_synced_) { setMessage("Waiting for the robot", now); return false; }
int t = trim_target_;
switch (e.value) {
case SERVO_TRIM_MINUS_BIG: t -= SERVO_TRIM_STEP_BIG_US; break;
case SERVO_TRIM_MINUS_SMALL: t -= SERVO_TRIM_STEP_SMALL_US; break;
case SERVO_TRIM_PLUS_SMALL: t += SERVO_TRIM_STEP_SMALL_US; break;
case SERVO_TRIM_PLUS_BIG: t += SERVO_TRIM_STEP_BIG_US; break;
case SERVO_TRIM_ZERO: t = 0; break;
default: return false;
}
trim_target_ = (int16_t)constrain(t, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
return true;
}
case UiCmd::ServoCalSave:
case UiCmd::ServoCalRevert:
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
cal_op_ = e.cmd == UiCmd::ServoCalSave ? CAL_SAVE : CAL_REVERT;
cal_op_until_ms_ = now + SERVO_CAL_OP_MS;
setMessage(e.cmd == UiCmd::ServoCalSave ? "Trims saved on the robot" : "Unsaved trims dropped", now);
return true;
case UiCmd::CalCancel: case UiCmd::CalCancel:
if (!calibrating_) return false; if (!calibrating_) return false;
stick_hw.cancelCalibration(); stick_hw.cancelCalibration();
@@ -91,16 +172,35 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
return false; return false;
} }
void RemoteState::update(float dt, bool link_ok, uint32_t now_ms) { void RemoteState::update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms) {
// Mirror the robot's link failsafe: it disarms (servos relax) when packets // Mirror the robot's link failsafe: it disarms (servos relax) when packets
// stop, so the remote disarms too and the operator must re-arm on purpose // stop, so the remote disarms too and the operator must re-arm on purpose
// once the link is back. // once the link is back. Servo calibration stops the same way, so the
if (!link_ok && link_was_ok_ && armed_) { // servos don't spring back to centre on their own when the link returns.
disarm(); if (!link_ok && link_was_ok_) {
setMessage("Link lost - disarmed", now_ms); if (armed_) {
disarm();
setMessage("Link lost - disarmed", now_ms);
}
if (servo_cal_) {
stopServoCal();
setMessage("Link lost - servo cal stopped", now_ms);
}
} }
link_was_ok_ = link_ok; link_was_ok_ = link_ok;
// SAVE / REVERT done: after a revert the robot's trims changed under us, so
// read the selected one back again.
if (cal_op_ != CAL_NONE && now_ms >= cal_op_until_ms_) {
if (cal_op_ == CAL_REVERT) trim_synced_ = false;
cal_op_ = CAL_NONE;
}
if (servo_cal_ && !trim_synced_ && cal_op_ == CAL_NONE && tele_fresh &&
tele.state == TSTATE_SERVO_CAL && tele.cal_servo == servo_sel_) {
trim_target_ = tele.cal_trim_us;
trim_synced_ = true;
}
speed_.setTarget(speed_target_); speed_.setTarget(speed_target_);
speed_.update(dt); speed_.update(dt);
tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f); tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f);
@@ -115,7 +215,12 @@ void RemoteState::fillView(UiView& v, uint32_t now_ms) const {
v.rstick_xlate = rstick_xlate_; v.rstick_xlate = rstick_xlate_;
v.remote_gyro = remote_gyro_; v.remote_gyro = remote_gyro_;
v.robot_gyro = robot_gyro_; v.robot_gyro = robot_gyro_;
v.calibrating = calibrating_; v.stick_calibrating = calibrating_;
v.servo_cal = servo_cal_;
v.servo_sel = servo_sel_;
v.trim_synced = trim_synced_;
v.trim_us = trim_target_;
v.upload_confirm = now_ms < upload_confirm_until_ms_;
v.speed_target = speed_target_; v.speed_target = speed_target_;
v.height = height_; v.height = height_;
v.message = message(now_ms); v.message = message(now_ms);
+28 -2
View File
@@ -17,8 +17,9 @@ public:
// rejections leave an explanation in message(). // rejections leave an explanation in message().
bool handle(const UiEvent& e, Sticks& sticks, RemoteIMU& imu, uint32_t now_ms); bool handle(const UiEvent& e, Sticks& sticks, RemoteIMU& imu, uint32_t now_ms);
// Advances smoothers and link-loss handling. Call once per TX tick. // Advances smoothers and link-loss handling, and reads the servo trim back
void update(float dt, bool link_ok, uint32_t now_ms); // from the robot's telemetry. Call once per TX tick.
void update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms);
bool armed() const { return armed_; } bool armed() const { return armed_; }
bool standRequested() const { return stand_; } bool standRequested() const { return stand_; }
@@ -33,6 +34,17 @@ public:
uint8_t speedCurrent() const { return (uint8_t)lroundf(speed_.current); } uint8_t speedCurrent() const { return (uint8_t)lroundf(speed_.current); }
float remoteTiltBlend() const { return tilt_blend_.current; } // 0..1, fades on the gyro toggle float remoteTiltBlend() const { return tilt_blend_.current; } // 0..1, fades on the gyro toggle
// Servo centre calibration (CAL page) -> CmdPacket.cal_*
uint8_t servoCalCmd(uint32_t now_ms) const;
uint8_t servoSel() const { return servo_sel_; } // leg * 3 + joint
int16_t trimTarget() const { return trim_target_; }
bool servoCal() const { return servo_cal_; }
bool trimSynced() const { return trim_synced_; }
uint8_t page() const { return page_; }
// UPDATE DISPLAY was confirmed: remote.ino enters the upload passthrough.
bool takeUploadRequest() { bool r = upload_go_; upload_go_ = false; return r; }
const char* message(uint32_t now_ms) const { return now_ms < message_until_ms_ ? message_ : ""; } const char* message(uint32_t now_ms) const { return now_ms < message_until_ms_ ? message_ : ""; }
void setMessage(const char* msg, uint32_t now_ms); void setMessage(const char* msg, uint32_t now_ms);
@@ -53,9 +65,23 @@ private:
SmoothedValue speed_{SPEED_SCALE_RATE_PER_SEC}; SmoothedValue speed_{SPEED_SCALE_RATE_PER_SEC};
SmoothedValue tilt_blend_{REMOTE_TILT_FADE_PER_SEC}; SmoothedValue tilt_blend_{REMOTE_TILT_FADE_PER_SEC};
uint8_t page_ = PAGE_UNKNOWN;
bool servo_cal_ = false;
uint8_t servo_sel_ = 0;
int16_t trim_target_ = 0;
bool trim_synced_ = false; // trim_target_ was read back from the robot for servo_sel_
uint8_t cal_op_ = CAL_NONE; // CAL_SAVE / CAL_REVERT while it's being sent
uint32_t cal_op_until_ms_ = 0;
uint32_t upload_confirm_until_ms_ = 0;
bool upload_go_ = false;
bool link_was_ok_ = false; bool link_was_ok_ = false;
const char* message_ = ""; const char* message_ = "";
uint32_t message_until_ms_ = 0; uint32_t message_until_ms_ = 0;
void disarm(); void disarm();
void stopServoCal();
void selectServo(uint8_t servo);
}; };
+9
View File
@@ -23,6 +23,15 @@ bool SerialConsole::poll(UiEvent& ui, Local& local) {
case 'c': ui.cmd = UiCmd::CalStartSave; return true; case 'c': ui.cmd = UiCmd::CalStartSave; return true;
case 'x': ui.cmd = UiCmd::CalCancel; return true; case 'x': ui.cmd = UiCmd::CalCancel; return true;
case 'U': ui.cmd = UiCmd::NextionUpload; return true; case 'U': ui.cmd = UiCmd::NextionUpload; return true;
case 'o': ui.cmd = UiCmd::ServoCalToggle; return true;
case 'n': ui.cmd = UiCmd::ServoCalNext; return true;
case ',': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_MINUS_SMALL }; return true;
case '.': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_PLUS_SMALL }; return true;
case '<': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_MINUS_BIG }; return true;
case '>': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_PLUS_BIG }; return true;
case '0': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_ZERO }; return true;
case 'w': ui.cmd = UiCmd::ServoCalSave; return true;
case 'e': ui.cmd = UiCmd::ServoCalRevert; return true;
case 'p': local = Local::PrintStatus; return true; case 'p': local = Local::PrintStatus; return true;
case 'd': local = Local::ToggleDump; return true; case 'd': local = Local::ToggleDump; return true;
case 'R': local = Local::ResetCal; return true; case 'R': local = Local::ResetCal; return true;
+50 -4
View File
@@ -1,5 +1,7 @@
#pragma once #pragma once
#include <Arduino.h> #include <Arduino.h>
#include <HexapodProtocol.h>
#include "Config.h"
// Operator commands. Produced by the Nextion touchscreen (NextionUI) and by // Operator commands. Produced by the Nextion touchscreen (NextionUI) and by
// the USB serial console (SerialConsole) through the same path, so the // the USB serial console (SerialConsole) through the same path, so the
@@ -21,19 +23,43 @@ enum class UiCmd : uint8_t {
HeightUp, // serial console only HeightUp, // serial console only
HeightDown, // serial console only HeightDown, // serial console only
ZeroGyro, // capture the handset's current attitude as "level" ZeroGyro, // capture the handset's current attitude as "level"
CalStartSave, // start sweep calibration, or save it if running CalStartSave, // stick sweep calibration: start, or save it if running
CalCancel, CalCancel,
NextionUpload, // USB <-> display passthrough for Nextion Editor (handled in remote.ino) PageShown, // value = Nextion page id (UiPage); the display just showed it
ServoCalToggle, // servo centre calibration on/off (robot holds the centre pose)
ServoCalLeg, // value = leg 0..5
ServoCalJoint, // value = joint 0..2 (coxa, femur, tibia)
ServoCalNext, // serial console only: next servo
ServoCalTrim, // value = SERVO_TRIM_* step code
ServoCalSave, // store the trims in the robot's EEPROM
ServoCalRevert, // reload the trims from the robot's EEPROM
UploadRequest, // display update mode from the touchscreen: needs a second tap
NextionUpload, // display update mode right away (serial console 'U')
}; };
// ServoCalTrim values.
constexpr uint8_t SERVO_TRIM_MINUS_BIG = 0;
constexpr uint8_t SERVO_TRIM_MINUS_SMALL = 1;
constexpr uint8_t SERVO_TRIM_PLUS_SMALL = 2;
constexpr uint8_t SERVO_TRIM_PLUS_BIG = 3;
constexpr uint8_t SERVO_TRIM_ZERO = 4;
struct UiEvent { struct UiEvent {
UiCmd cmd = UiCmd::None; UiCmd cmd = UiCmd::None;
uint8_t value = 0; uint8_t value = 0;
}; };
// The HMI's pages (remote/remote.HMI, page id = position in the Editor's
// page list). Each page reports itself with a 'P' frame when it opens.
enum UiPage : uint8_t {
PAGE_SPLASH = 0, PAGE_DRIVE, PAGE_STATUS, PAGE_SERVOS, PAGE_CAL, PAGE_SETUP, PAGE_UPLOAD,
PAGE_UNKNOWN = 0xFF,
};
// Everything the display shows. The remote is authoritative: the Nextion // Everything the display shows. The remote is authoritative: the Nextion
// only sends requests, and always renders what's in here. // only sends requests, and always renders what's in here.
struct UiView { struct UiView {
// Operator state (DRIVE page)
bool armed = false; bool armed = false;
bool stand = false; bool stand = false;
bool gait_wave = false; bool gait_wave = false;
@@ -41,10 +67,30 @@ struct UiView {
bool rstick_xlate = false; bool rstick_xlate = false;
bool remote_gyro = false; bool remote_gyro = false;
bool robot_gyro = false; bool robot_gyro = false;
bool calibrating = false;
uint8_t speed_target = 0; uint8_t speed_target = 0;
int16_t height = 0; // -1000..1000, 0 = default int16_t height = 0; // -1000..1000, 0 = default
// Radio + robot telemetry
bool link_ok = false; bool link_ok = false;
uint8_t link_pct = 0; uint8_t link_pct = 0;
const char* message = ""; // always a string literal - NextionUI diffs it by pointer bool tele_ok = false; // fresh telemetry from the robot
Telemetry tele;
// Handset (STATUS page)
bool imu_ok = false;
float imu_roll = 0, imu_pitch = 0; // degrees, vs the zeroed level
int16_t sticks[NUM_AXES] = {};
uint16_t battery_mv = 0; // 0 = not measured
// SETUP page
bool stick_calibrating = false;
bool upload_confirm = false; // UPDATE DISPLAY tapped once, waiting for the second tap
// CAL page (servo centre calibration)
bool servo_cal = false;
uint8_t servo_sel = 0; // leg * 3 + joint
bool trim_synced = false; // trim below is known (read back from the robot)
int16_t trim_us = 0;
const char* message = "";
}; };
Binary file not shown.
+30 -3
View File
@@ -6,6 +6,7 @@
#include "RadioLink.h" #include "RadioLink.h"
#include "NextionUI.h" #include "NextionUI.h"
#include "SerialConsole.h" #include "SerialConsole.h"
#include "Battery.h"
// Hexapod remote (Teensy 4.0): reads two 3-axis sticks and the handset // Hexapod remote (Teensy 4.0): reads two 3-axis sticks and the handset
// MPU6050, takes operator commands from the Nextion touchscreen (and the USB // MPU6050, takes operator commands from the Nextion touchscreen (and the USB
@@ -19,6 +20,7 @@ PacketBuilder builder;
RadioLink radio_link; RadioLink radio_link;
NextionUI nextion; NextionUI nextion;
SerialConsole console; SerialConsole console;
Battery battery;
uint32_t last_tx_us = 0; uint32_t last_tx_us = 0;
bool dump_packets = false; bool dump_packets = false;
@@ -39,6 +41,22 @@ static void printStatus() {
state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot", state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot",
state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent(), state.height()); state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent(), state.height());
sticks.printCalibration(Serial); sticks.printCalibration(Serial);
if (BATTERY_SENSE_PIN != 255) Serial.printf("battery %u mV\n", battery.millivolts());
const Telemetry& t = radio_link.telemetry();
if (radio_link.telemetryFresh(millis())) {
Serial.printf("robot: state %u status %02X roll %.1f pitch %.1f battery %u mV, hears %u/s\n",
t.state, t.status, t.roll_ddeg / 10.0f, t.pitch_ddeg / 10.0f, t.battery_mv, t.rx_per_sec);
Serial.print("robot joints (deg):");
for (int i = 0; i < 18; i++)
Serial.printf(" %d", t.joint_deg[i] - (i % 3 == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0));
Serial.println();
} else {
Serial.println("robot: no telemetry");
}
Serial.printf("servo cal %s, servo %u (leg %u joint %u) trim %d us%s, robot reports servo %u trim %d\n",
state.servoCal() ? "ON" : "off", state.servoSel(), state.servoSel() / 3, state.servoSel() % 3,
state.trimTarget(), state.trimSynced() ? "" : " (not read back yet)", t.cal_servo, t.cal_trim_us);
} }
static void dumpPacket(const CmdPacket& p) { static void dumpPacket(const CmdPacket& p) {
@@ -47,7 +65,7 @@ static void dumpPacket(const CmdPacket& p) {
p.remote_roll, p.remote_pitch, p.body_height, p.speed_scale, p.flags); p.remote_roll, p.remote_pitch, p.body_height, p.speed_scale, p.flags);
} }
// Nextion Editor upload through this USB port (the old remote's b2 button). // Nextion Editor upload through this USB port (SETUP > UPDATE DISPLAY, or 'U').
// Never returns: the TX loop stops, so the robot loses the link and // Never returns: the TX loop stops, so the robot loses the link and
// disarms itself after LINK_TIMEOUT_SEC. Power-cycle the remote to leave. // disarms itself after LINK_TIMEOUT_SEC. Power-cycle the remote to leave.
static void enterNextionUpload() { static void enterNextionUpload() {
@@ -61,8 +79,8 @@ static void enterNextionUpload() {
static void pollInputs(uint32_t now_ms) { static void pollInputs(uint32_t now_ms) {
UiEvent ev; UiEvent ev;
while (nextion.poll(ev)) { while (nextion.poll(ev)) {
if (ev.cmd == UiCmd::NextionUpload) enterNextionUpload();
state.handle(ev, sticks, imu, now_ms); state.handle(ev, sticks, imu, now_ms);
if (state.takeUploadRequest()) enterNextionUpload(); // UPDATE DISPLAY, tapped twice
} }
SerialConsole::Local local; SerialConsole::Local local;
@@ -114,7 +132,9 @@ void loop() {
sticks.update(); sticks.update();
imu.update(dt); imu.update(dt);
radio_link.update(now_ms); radio_link.update(now_ms);
state.update(dt, radio_link.linkOk(), now_ms); battery.update(dt);
bool tele_fresh = radio_link.telemetryFresh(now_ms);
state.update(dt, radio_link.linkOk(), tele_fresh, radio_link.telemetry(), now_ms);
last_packet = builder.build(sticks.frame(), imu, state, now_ms); last_packet = builder.build(sticks.frame(), imu, state, now_ms);
radio_link.send(last_packet); radio_link.send(last_packet);
@@ -123,6 +143,13 @@ void loop() {
state.fillView(view, now_ms); state.fillView(view, now_ms);
view.link_ok = radio_link.linkOk(); view.link_ok = radio_link.linkOk();
view.link_pct = radio_link.qualityPct(); view.link_pct = radio_link.qualityPct();
view.tele_ok = tele_fresh;
view.tele = radio_link.telemetry();
view.imu_ok = imu.ok();
view.imu_roll = imu.rollDeg();
view.imu_pitch = imu.pitchDeg();
for (int i = 0; i < NUM_AXES; i++) view.sticks[i] = sticks.frame().v[i];
view.battery_mv = battery.millivolts();
nextion.render(view, now_ms); nextion.render(view, now_ms);
if (dump_packets && now_ms - last_dump_ms >= 100) { if (dump_packets && now_ms - last_dump_ms >= 100) {
+10
View File
@@ -0,0 +1,10 @@
#include "Battery.h"
#include "Config.h"
void Battery::update(float dt) {
if (BATTERY_SENSE_PIN == 255) return; // not fitted: stays 0
float v = analogRead(BATTERY_SENSE_PIN) * (BATTERY_ADC_REF_V / BATTERY_ADC_MAX) * BATTERY_DIVIDER;
if (!primed_) { volts_ = v; primed_ = true; return; }
float a = constrain(dt / 2.0f, 0.0f, 1.0f); // ~2 s time constant: servo load makes it sag and bounce
volts_ += (v - volts_) * a;
}
+13
View File
@@ -0,0 +1,13 @@
#pragma once
#include <Arduino.h>
// Battery voltage through a resistor divider (Config.h BATTERY_*), smoothed.
class Battery {
public:
void update(float dt);
uint16_t millivolts() const { return (uint16_t)(volts_ * 1000.0f + 0.5f); } // 0 = not fitted
private:
float volts_ = 0;
bool primed_ = false;
};
+29 -3
View File
@@ -12,22 +12,48 @@ bool Comms::begin() {
SPI.setSCK(RADIO_SCK_PIN); // alternate SCK pin (see Config.h); before RF24 starts SPI SPI.setSCK(RADIO_SCK_PIN); // alternate SCK pin (see Config.h); before RF24 starts SPI
radio_present_ = radio.begin(); radio_present_ = radio.begin();
if (radio_present_) { if (radio_present_) {
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all // Telemetry rides back on the auto-ack (ack payloads), which needs dynamic
// payload lengths - update() drops any packet that isn't a CmdPacket.
radio.enableAckPayload();
radio.openReadingPipe(1, HXP_RADIO_ADDRESS); radio.openReadingPipe(1, HXP_RADIO_ADDRESS);
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL); radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE); radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
radio.setChannel(HXP_RADIO_CHANNEL); radio.setChannel(HXP_RADIO_CHANNEL);
radio.startListening(); radio.startListening();
loadAckPayload();
} }
return radio_present_; return radio_present_;
} }
// Replaces whatever ack payload is still queued with the latest telemetry, so
// the remote never gets stale data.
void Comms::loadAckPayload() {
radio.flush_tx();
radio.writeAckPayload(1, &telemetry_, sizeof(telemetry_));
}
void Comms::update() { void Comms::update() {
if (radio_present_) { if (radio_present_) {
bool got = false;
while (radio.available()) { while (radio.available()) {
radio.read(&packet_, sizeof(packet_)); uint8_t len = radio.getDynamicPayloadSize();
last_rx_ms_ = millis(); if (len == sizeof(CmdPacket)) {
radio.read(&packet_, sizeof(packet_));
last_rx_ms_ = millis();
rx_count_++;
got = true;
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
uint8_t junk[32]; // another protocol version: never act on it
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
}
} }
if (got) loadAckPayload();
}
uint32_t now = millis();
if (now - rate_window_ms_ >= 1000) {
rx_per_sec_ = (uint8_t)min(rx_count_, (uint16_t)255);
rx_count_ = 0;
rate_window_ms_ = now;
} }
link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000); link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000);
} }
+11
View File
@@ -12,9 +12,20 @@ public:
// Lets the serial test harness (or, later, real radio) inject a packet directly. // Lets the serial test harness (or, later, real radio) inject a packet directly.
void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); } void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); }
// Telemetry for the remote. Goes out as the ack payload of the next packet
// received, so set it every loop.
void setTelemetry(const Telemetry& t) { telemetry_ = t; }
uint8_t packetsPerSec() const { return rx_per_sec_; }
private: private:
CmdPacket packet_; CmdPacket packet_;
uint32_t last_rx_ms_ = 0; uint32_t last_rx_ms_ = 0;
bool link_ok_ = false; bool link_ok_ = false;
bool radio_present_ = false; bool radio_present_ = false;
Telemetry telemetry_;
uint16_t rx_count_ = 0;
uint8_t rx_per_sec_ = 0;
uint32_t rate_window_ms_ = 0;
void loadAckPayload();
}; };
+9
View File
@@ -140,6 +140,15 @@ constexpr uint8_t RADIO_CSN_PIN = 15;
constexpr uint8_t RADIO_SCK_PIN = 14; constexpr uint8_t RADIO_SCK_PIN = 14;
constexpr float LINK_TIMEOUT_SEC = 0.3f; constexpr float LINK_TIMEOUT_SEC = 0.3f;
// ---- Battery voltage (shown on the remote's STATUS page) ----
// Battery + through a resistor divider into an analog pin. BENCH: no divider
// is wired yet - set the pin and the divider ratio (battery V / pin V) once
// it is. 255 = not fitted: the remote then shows "--".
constexpr uint8_t BATTERY_SENSE_PIN = 255;
constexpr float BATTERY_DIVIDER = 3.0f; // e.g. 20k over 10k
constexpr float BATTERY_ADC_REF_V = 3.3f; // Teensy 3.6 analog reference
constexpr int BATTERY_ADC_MAX = 1023; // default 10-bit analogRead()
// ---- Robot-gyro self-leveling ---- // ---- Robot-gyro self-leveling ----
// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by // Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot. // this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
+19 -1
View File
@@ -116,9 +116,27 @@ ignore walk commands.
## Link with the remote ## Link with the remote
- `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the - `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the
exact wire format. Both radios set a static payload size of exact wire format. `Comms` drops any packet whose length isn't
`sizeof(CmdPacket)`, so a robot and remote built from different protocol `sizeof(CmdPacket)`, so a robot and remote built from different protocol
versions won't receive each other at all (and the robot fails safe). versions won't receive each other at all (and the robot fails safe).
- **Telemetry back to the remote:** after every packet received, `Comms`
loads a fresh `Telemetry` (robot state, IMU roll/pitch, battery, the 18
commanded joint angles, packets per second, servo-cal echo) as the radio's
ack payload. It goes back on the auto-ack of the remote's next packet:
no extra transmissions. `sendTelemetry()` in `robot.ino` fills it every
loop. The IMU attitude is now estimated whether or not robot gyro is on
(the remote shows it); only the leveling is gated by `FLAG_ROBOT_GYRO`.
- **Servo centre calibration** (remote's CAL page): while disarmed, a
`cal_cmd` other than `CAL_NONE` puts the robot in SERVO_CAL. Every servo
is driven to the centre pose (coxa 0, femur 0, tibia −90: legs straight
out, shins straight down) and `CAL_SET` adjusts one servo's centre trim
(µs, ±200, on top of `Config.h` `SERVOS`; it shifts the whole pulse range
like the old `servoCallibration[]`). `CAL_SAVE` stores the 18 trims in
EEPROM (`ServoDriver`, address 0, loaded at power-on), `CAL_REVERT`
reloads them. `CAL_NONE`, arming or link loss relaxes the servos again.
Put the robot on a stand first.
- **Battery:** `Battery` reads `BATTERY_SENSE_PIN` through a divider
(`Config.h`). Not wired yet (pin 255): the remote shows "--".
- Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()` - Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()`
goes false and the main loop falls back to a zeroed `CmdPacket`, which goes false and the main loop falls back to a zeroed `CmdPacket`, which
disarms the robot. disarms the robot.
+1 -1
View File
@@ -38,7 +38,7 @@ bool RobotIMU::readRaw(float& ax, float& ay, float& az, float& gx, float& gy, fl
} }
void RobotIMU::update(float dt) { void RobotIMU::update(float dt) {
if (!ok_ || !enabled) return; if (!ok_) return;
float ax, ay, az, gx, gy, gz; float ax, ay, az, gx, gy, gz;
if (!readRaw(ax, ay, az, gx, gy, gz)) return; if (!readRaw(ax, ay, az, gx, gy, gz)) return;
+5 -2
View File
@@ -4,11 +4,14 @@
class RobotIMU { class RobotIMU {
public: public:
bool begin(); // returns false if the IMU didn't respond bool begin(); // returns false if the IMU didn't respond
void update(float dt); // no-op if disabled or the IMU never came up void update(float dt); // no-op if the IMU never came up
float rollDeg() const { return roll_deg_; } float rollDeg() const { return roll_deg_; }
float pitchDeg() const { return pitch_deg_; } float pitchDeg() const { return pitch_deg_; }
bool ok() const { return ok_; }
bool enabled = true; // robot-gyro toggle from CmdPacket sets this // Robot-gyro toggle from CmdPacket. The attitude is estimated either way
// (the remote shows it); robot.ino only levels the body while it's set.
bool enabled = true;
private: private:
float roll_deg_ = 0, pitch_deg_ = 0; float roll_deg_ = 0, pitch_deg_ = 0;
+71 -2
View File
@@ -1,5 +1,7 @@
#include "ServoDriver.h" #include "ServoDriver.h"
#include "Config.h" #include "Config.h"
#include <HexapodProtocol.h>
#include <EEPROM.h>
// 18 servos on hardware PWM, the way the old robot sketch drove them (the // 18 servos on hardware PWM, the way the old robot sketch drove them (the
// Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per // Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per
@@ -28,10 +30,77 @@ void ServoDriver::relaxAll() {
analogWrite(SERVOS[i][j].pin, 0); // no pulses: the servos go limp analogWrite(SERVOS[i][j].pin, 0); // no pulses: the servos go limp
} }
// ---- Centre trims ----
// EEPROM record at address 0 (nothing else on the robot uses EEPROM). Bump
// TRIM_VERSION if the layout changes - an old record is then ignored.
static constexpr int TRIM_EEPROM_ADDR = 0;
static constexpr uint32_t TRIM_MAGIC = 0x52545848; // "HXTR"
static constexpr uint8_t TRIM_VERSION = 1;
struct TrimRecord {
uint32_t magic;
uint8_t version;
uint8_t checksum; // sum of the trim bytes
int16_t trim_us[NUM_LEGS * 3];
};
static int16_t g_trim[NUM_LEGS * 3] = {};
static bool g_dirty = false;
static bool g_stored = false;
static uint8_t trimChecksum(const int16_t* t) {
uint8_t sum = 0;
const uint8_t* b = reinterpret_cast<const uint8_t*>(t);
for (size_t i = 0; i < sizeof(int16_t) * NUM_LEGS * 3; i++) sum += b[i];
return sum;
}
void ServoDriver::loadTrims() {
TrimRecord r;
EEPROM.get(TRIM_EEPROM_ADDR, r);
g_stored = r.magic == TRIM_MAGIC && r.version == TRIM_VERSION && r.checksum == trimChecksum(r.trim_us);
for (int i = 0; i < NUM_LEGS * 3; i++)
g_trim[i] = g_stored ? constrain(r.trim_us[i], -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US) : 0;
g_dirty = false;
}
static void saveTrims() {
TrimRecord r;
r.magic = TRIM_MAGIC;
r.version = TRIM_VERSION;
memcpy(r.trim_us, g_trim, sizeof(g_trim));
r.checksum = trimChecksum(r.trim_us);
EEPROM.put(TRIM_EEPROM_ADDR, r);
g_dirty = false;
g_stored = true;
}
int16_t ServoDriver::trim(int servo) { return (servo >= 0 && servo < NUM_LEGS * 3) ? g_trim[servo] : 0; }
bool ServoDriver::trimsDirty() { return g_dirty; }
bool ServoDriver::trimsStored() { return g_stored; }
const JointAngles ServoDriver::CAL_POSE = { 0.0f, 0.0f, -90.0f, true };
void ServoDriver::calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us) {
bool valid_servo = servo < NUM_LEGS * 3;
switch (cal_cmd) {
case CAL_SET:
if (valid_servo) {
int16_t v = constrain(value_us, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
if (v != g_trim[servo]) { g_trim[servo] = v; g_dirty = true; }
}
break;
case CAL_SAVE: if (g_dirty) saveTrims(); break; // the remote repeats it: only write once
case CAL_REVERT: if (g_dirty) loadTrims(); break;
default: break; // CAL_HOLD
}
for (int i = 0; i < NUM_LEGS; i++) writeLeg(i, CAL_POSE);
}
int ServoDriver::degToMicros(int leg, int joint, float deg) { int ServoDriver::degToMicros(int leg, int joint, float deg) {
const ServoCal& c = SERVOS[leg][joint]; const ServoCal& c = SERVOS[leg][joint];
float us = c.us_at_0deg + c.us_per_deg * deg; int16_t t = g_trim[leg * 3 + joint];
return (int)constrain(us, (float)c.us_min, (float)c.us_max); float us = c.us_at_0deg + t + c.us_per_deg * deg;
return (int)constrain(us, (float)(c.us_min + t), (float)(c.us_max + t));
} }
void ServoDriver::writeLeg(int i, const JointAngles& a) { void ServoDriver::writeLeg(int i, const JointAngles& a) {
+15 -1
View File
@@ -7,6 +7,20 @@ public:
void relaxAll(); // stop all pulses - call when disarming void relaxAll(); // stop all pulses - call when disarming
void writeLeg(int leg_index, const JointAngles& angles); void writeLeg(int leg_index, const JointAngles& angles);
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS). // Joint angle -> pulse through that servo's calibration (Config.h SERVOS)
// plus its centre trim.
static int degToMicros(int leg, int joint, float deg); static int degToMicros(int leg, int joint, float deg);
// ---- Servo centre calibration (remote's CAL page, CmdPacket.cal_*) ----
// Each servo has a centre trim in us on top of Config.h SERVOS, kept in
// EEPROM. Trims shift the whole pulse range, like the old sketch's
// servoCallibration[].
static void loadTrims(); // from EEPROM; all 0 if none stored. Call in setup()
static int16_t trim(int servo); // servo = leg * 3 + joint
static bool trimsDirty(); // changed since the last load/save
static bool trimsStored(); // the EEPROM holds a trim record
// Applies one calibration command (CAL_* in HexapodProtocol.h) and holds
// every servo at the centre pose. Call every loop while in SERVO_CAL.
void calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us);
static const JointAngles CAL_POSE; // coxa 0, femur 0, tibia -90
}; };
+46
View File
@@ -9,6 +9,7 @@
#include "Comms.h" #include "Comms.h"
#include "ServoDriver.h" #include "ServoDriver.h"
#include "SmoothedValue.h" #include "SmoothedValue.h"
#include "Battery.h"
// Uncomment to drive the robot from the Serial Monitor before a remote exists. // Uncomment to drive the robot from the Serial Monitor before a remote exists.
// See README.md for the key map. // See README.md for the key map.
@@ -21,6 +22,7 @@ StateMachine state_machine;
InputMapper mapper; InputMapper mapper;
Comms comms; Comms comms;
ServoDriver servo_driver; ServoDriver servo_driver;
Battery battery;
float current_heading_deg = 0; // open-loop: accumulates commanded omega. See note in loop(). float current_heading_deg = 0; // open-loop: accumulates commanded omega. See note in loop().
SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S); // remote height slider, rate-limited SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S); // remote height slider, rate-limited
@@ -28,6 +30,7 @@ float gyro_corr_roll_deg = 0, gyro_corr_pitch_deg = 0; // robot-gyro self-level
float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
uint32_t last_loop_us = 0; uint32_t last_loop_us = 0;
RobotState last_state = RobotState::DISARMED; RobotState last_state = RobotState::DISARMED;
bool servo_cal_active = false; // SERVO_CAL: servos held at the centre pose for trimming
#ifdef TEST_SERIAL_INPUT #ifdef TEST_SERIAL_INPUT
CmdPacket test_packet; CmdPacket test_packet;
@@ -63,6 +66,33 @@ void pollSerialTestInput() {
} }
#endif #endif
// Robot state for the remote's display, sent back in the radio's ack payload.
void sendTelemetry(const CmdPacket& cmd, const MapperOutput& mapped, RobotState state) {
Telemetry t;
t.roll_ddeg = (int16_t)lroundf(constrain(imu.rollDeg(), -3000.0f, 3000.0f) * 10);
t.pitch_ddeg = (int16_t)lroundf(constrain(imu.pitchDeg(), -3000.0f, 3000.0f) * 10);
t.battery_mv = battery.millivolts();
t.state = servo_cal_active ? TSTATE_SERVO_CAL : (uint8_t)state;
t.status = (imu.ok() ? TSTAT_IMU_OK : 0) |
(mapped.robot_gyro_enabled ? TSTAT_GYRO_ON : 0) |
(state != RobotState::DISARMED || servo_cal_active ? TSTAT_SERVOS_ON : 0) |
(ServoDriver::trimsDirty() ? TSTAT_CAL_DIRTY : 0) |
(ServoDriver::trimsStored() ? TSTAT_TRIMS_STORED : 0);
t.cal_servo = cmd.cal_servo;
t.cal_trim_us = ServoDriver::trim(cmd.cal_servo);
t.rx_per_sec = comms.packetsPerSec();
for (int i = 0; i < NUM_LEGS; i++) {
const float* j = last_joint_deg[i];
float c = servo_cal_active ? ServoDriver::CAL_POSE.coxa_deg : j[0];
float f = servo_cal_active ? ServoDriver::CAL_POSE.femur_deg : j[1];
float b = servo_cal_active ? ServoDriver::CAL_POSE.tibia_deg : j[2];
t.joint_deg[i * 3] = (int8_t)constrain(lroundf(c), -127L, 127L);
t.joint_deg[i * 3 + 1] = (int8_t)constrain(lroundf(f), -127L, 127L);
t.joint_deg[i * 3 + 2] = (int8_t)constrain(lroundf(b) + TELEMETRY_TIBIA_OFFSET_DEG, -127L, 127L);
}
comms.setTelemetry(t);
}
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
gait.begin(); gait.begin();
@@ -73,6 +103,7 @@ void setup() {
if (!imu.begin()) Serial.println("robot IMU not responding - stabilization disabled"); if (!imu.begin()) Serial.println("robot IMU not responding - stabilization disabled");
if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only"); if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only");
ServoDriver::loadTrims();
servo_driver.begin(); servo_driver.begin();
last_loop_us = micros(); last_loop_us = micros();
} }
@@ -115,6 +146,18 @@ void loop() {
} else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) { } else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
servo_driver.begin(); servo_driver.begin();
} }
// Servo centre calibration (remote CAL page): only while disarmed. The
// servos hold the centre pose through their trims; leaving it - CAL_NONE,
// arming or link loss (cmd is then a default packet) - relaxes them again.
bool servo_cal = !mapped.armed && cmd.cal_cmd != CAL_NONE;
if (servo_cal) {
if (!servo_cal_active) servo_driver.begin();
servo_driver.calibrate(cmd.cal_cmd, cmd.cal_servo, cmd.cal_value);
} else if (servo_cal_active && state == RobotState::DISARMED) {
servo_driver.relaxAll();
}
servo_cal_active = servo_cal;
// The stand pose holds every foot at neutral, so walking starts from there. // The stand pose holds every foot at neutral, so walking starts from there.
if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet(); if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet();
last_state = state; last_state = state;
@@ -203,4 +246,7 @@ void loop() {
servo_driver.writeLeg(i, angles); servo_driver.writeLeg(i, angles);
} }
} }
battery.update(dt);
sendTelemetry(cmd, mapped, state);
} }
+267
View File
@@ -0,0 +1,267 @@
"""Builds remote/remote.HMI, the remote's Nextion touchscreen project, from code.
The layout below is the single source of truth for the HMI: change it here and
re-run, rather than editing remote.HMI in Nextion Editor (an Editor edit is
lost on the next run). Component names and event code must match the
firmware - remote/NextionUI.cpp and remote/NEXTION.md.
python scripts\\make_remote_hmi.py (from 2026-current, PowerShell 5.1 fine)
Needs:
- the nxhmi package: the tools/nextion-hmi-tools submodule (git submodule update --init)
- Pillow, and the DejaVu Sans Mono fonts (free licence). They ship with
matplotlib; or pass --font-dir with DejaVuSansMono.ttf + DejaVuSansMono-Bold.ttf.
- the old remote HMI (../2019-teensy-nrf24/remote-nextion/remote.HMI), read
only: its splash picture is reused.
Then open remote.HMI in Nextion Editor, compile and upload (SETUP > UPDATE
DISPLAY puts the remote in USB passthrough for that).
"""
import argparse
import os
import sys
import tempfile
HERE = os.path.dirname(os.path.abspath(__file__))
ROOT = os.path.dirname(HERE) # 2026-current
sys.path.insert(0, os.path.join(ROOT, "tools", "nextion-hmi-tools", "src"))
from nxhmi import mcp_server as nx # noqa: E402 (the same functions the nxhmi MCP server exposes)
OUT = os.path.join(ROOT, "remote", "remote.HMI")
OLD_HMI = os.path.join(ROOT, "..", "2019-teensy-nrf24", "remote-nextion", "remote.HMI")
W, H = 480, 320
BAR_H = 32 # top bar
TAB_Y, TAB_H = 272, 48
BIG, SMALL = 0, 1 # font ids: 24 px bold, 16 px
# Colours. The firmware repaints the stateful ones (remote/NextionUI.cpp):
# gray #c6c3c6 = off, green #00fc00 = on, red #f80000, orange #f88000.
PAGE_BG = "#182030"
BAR_BG = "#000000"
TEXT = "#ffffff"
LABEL = "#a0b0c8"
BTN = "#c6c3c6"
RED = "#f80000"
TRACK = "#404850"
TAB_ON = "#3060c0"
# Page order = page id; must match UiPage in remote/UiTypes.h.
PAGES = ["splash", "drive", "status", "servos", "cal", "setup", "upload"]
TABS = [("drive", "DRIVE"), ("status", "STATUS"), ("servos", "SERVOS"), ("cal", "CAL"), ("setup", "SETUP")]
LEG_NAMES = ["FR", "MR", "RR", "RL", "ML", "FL"] # robot leg order (robot/Config.h)
LEG_TITLES = ["FRONT RIGHT", "MID RIGHT", "REAR RIGHT", "REAR LEFT", "MID LEFT", "FRONT LEFT"]
def frame(cmd, value=None):
"""printh line for a custom '#' frame (remote/NextionUI.h)."""
body = [ord(cmd)] + ([value] if value is not None else [])
return "printh 23 %02X %s" % (len(body), " ".join("%02X" % b for b in body))
class Builder:
def __init__(self, path):
self.path = path
def add(self, page, kind, name, events=None, **att):
nx.t_add_component(self.path, page, kind, name, att, events or {})
def text(self, page, name, x, y, w, h, txt, font=SMALL, pco=TEXT, bco=PAGE_BG, xcen="left", ycen="center",
maxl=None, **kw):
self.add(page, "text", name, x=x, y=y, w=w, h=h, txt=txt, font=font, pco=pco, bco=bco,
xcen=xcen, ycen=ycen, txt_maxl=maxl or max(len(txt), 10), **kw)
def button(self, page, name, x, y, w, h, txt, up=None, font=BIG, bco=BTN, pco="#000000", maxl=None, **kw):
events = {"codesup": up} if up else {}
self.add(page, "button", name, events, x=x, y=y, w=w, h=h, txt=txt, font=font, bco=bco, bco2=bco,
pco=pco, pco2=pco, txt_maxl=maxl or max(len(txt), 10), **kw)
def slider(self, page, name, x, y, w, h, maxval, val, events=None, cursor=None):
self.add(page, "slider", name, events, x=x, y=y, w=w, h=h, bco=TRACK, bco1=TRACK, pco=TEXT,
wid=cursor or h, hig=h, maxval=maxval, minval=0, val=val)
def bar(self, page, name, x, y, w, h, val=0):
self.add(page, "progress_bar", name, x=x, y=y, w=w, h=h, bco=TRACK, pco="#00c000", val=val)
# ---- shared frame: top bar + tabs on every working page ----
def chrome(self, page):
self.text(page, "tLink", 0, 0, 112, BAR_H, "NO LINK", font=SMALL, pco=RED, bco=BAR_BG, xcen="center", maxl=12)
self.text(page, "tState", 112, 0, 256, BAR_H, "ROBOT: NO DATA", font=SMALL, pco=TEXT, bco=BAR_BG, maxl=30)
self.button(page, "bKill", 368, 0, 112, BAR_H, "DISARM", [frame("K")], font=BIG, bco=RED, pco=TEXT)
tw = W // len(TABS)
for i, (target, label) in enumerate(TABS):
on = target == page
self.button(page, "bTab%d" % i, i * tw, TAB_Y, tw, TAB_H, label, ["page %s" % target], font=SMALL,
bco=TAB_ON if on else "#283040", pco=TEXT, borderw=1)
def page(self, name, preinit=()):
"""Every page reports itself to the remote when it opens ('P' + page id)."""
if name != PAGES[0]:
nx.t_add_page(self.path, name, {"bco": PAGE_BG, "sta": "solid color"})
else:
nx.t_set_attributes(self.path, name, name, {"bco": PAGE_BG})
nx.t_set_event(self.path, name, name, "codesload", [frame("P", PAGES.index(name))] + list(preinit))
def splash_png(tmp):
"""The old remote's splash picture (a jumping spider), rendered from the old HMI."""
from nxhmi.render import Renderer
from nxhmi.container import HMIFile
img = Renderer(HMIFile.load(OLD_HMI)).render("splashsceen")
path = os.path.join(tmp, "splash.png")
img.save(path)
return path
def font_dir(arg):
if arg:
return arg
import matplotlib
return os.path.join(os.path.dirname(matplotlib.__file__), "mpl-data", "fonts", "ttf")
def build(path, fonts):
b = Builder(path)
program = ("//Runs once at power-on, before the first page.\r\n"
"bauds=115200 //the remote's NEXTION_BAUD; stored in the display\r\n"
"page 0\r\n")
from nxhmi import project
project.new_project("NX4832T035_011", PAGES[0], program=program).save(path)
nx.t_add_font(path, 24, os.path.join(fonts, "DejaVuSansMono-Bold.ttf"), "iso-8859-1") # font 0
nx.t_add_font(path, 16, os.path.join(fonts, "DejaVuSansMono.ttf"), "iso-8859-1") # font 1
with tempfile.TemporaryDirectory() as tmp:
pic = nx.t_add_picture(path, splash_png(tmp))["picture_id"]
# ---------------------------------------------------------------- 0 splash
p = "splash"
b.page(p)
nx.t_set_attributes(path, p, p, {"sta": "image", "pic": pic})
b.text(p, "t0", 0, 0, 480, 40, "HEXAPOD REMOTE", font=BIG, pco=TEXT, bco="#000000", xcen="center", sta="crop image", picc=pic)
b.add(p, "timer", "tm0", {"codestimer": ["page drive"]}, tim=1500, en="enable")
for name in PAGES[1:]:
b.page(name, preinit=PREINIT.get(name, ()))
for name, _ in TABS:
b.chrome(name)
# ---------------------------------------------------------------- 1 drive
p = "drive"
y = 40
for i, (n, t, c) in enumerate([("bArm", "DISARMED", "A"), ("bStand", "SIT", "S"),
("bGait", "TRIPOD", "G"), ("bMode", "DIRECT", "M")]):
b.button(p, n, 4 + i * 119, y, 115, 48, t, [frame(c)], maxl=10)
y = 94
for i, (n, t, c) in enumerate([("bRs", "R: ROTATE", "X"), ("bRGyro", "REMOTE GYRO", "R"), ("bBGyro", "ROBOT GYRO", "B")]):
b.button(p, n, 4 + i * 159, y, 154, 44, t, [frame(c)], font=SMALL, maxl=12)
b.text(p, "tSpL", 4, 146, 80, 36, "SPEED", font=SMALL, pco=LABEL)
send_speed = ["printh 23 02 56", "prints hSpeed.val,1"] # 'V' + value byte
b.slider(p, "hSpeed", 86, 150, 300, 28, 255, 128, {"codesslide": send_speed, "codesup": send_speed}, cursor=20)
b.text(p, "tSpeed", 394, 146, 82, 36, "50%", font=BIG, xcen="right", maxl=6)
b.text(p, "tHtL", 4, 190, 80, 36, "HEIGHT", font=SMALL, pco=LABEL)
send_height = ["printh 23 02 48", "prints hHeight.val,1"] # 'H' + value byte
b.slider(p, "hHeight", 86, 194, 300, 28, 200, 100, {"codesslide": send_height, "codesup": send_height}, cursor=20)
b.text(p, "tHeight", 394, 190, 82, 36, "+0%", font=BIG, xcen="right", maxl=6)
b.text(p, "tMsg", 4, 234, 472, 34, "", font=SMALL, pco="#ffd040", maxl=40)
# ---------------------------------------------------------------- 2 status
p = "status"
for side, x, prefix in (("REMOTE (deg)", 4, "R"), ("ROBOT (deg)", 248, "B")):
b.text(p, "tHdr" + prefix, x, 36, 228, 22, side, font=SMALL, pco=LABEL)
b.text(p, "t%sRoll" % prefix, x, 58, 228, 22, "roll --", maxl=16)
b.slider(p, "h%sRoll" % prefix, x, 80, 228, 16, 90, 45, cursor=10)
b.text(p, "t%sPitch" % prefix, x, 100, 228, 22, "pitch --", maxl=16)
b.slider(p, "h%sPitch" % prefix, x, 122, 228, 16, 90, 45, cursor=10)
b.text(p, "tHdrS", 4, 144, 60, 22, "STICKS", pco=LABEL)
for i, axis in enumerate(("X", "Y", "TWIST")): # centred over the slider columns below
x = 32 + i * 150
if i == 0:
x, w = 67, 70 # same centre (102), clear of "STICKS"
else:
w = 140
b.text(p, "tAx%d" % i, x, 144, w, 22, axis, pco=LABEL, xcen="center")
for row, (label, y) in enumerate((("L", 168), ("R", 192))):
b.text(p, "tSt" + label, 4, y, 24, 20, label, pco=LABEL)
for i, axis in enumerate("XYT"):
b.slider(p, "h%s%s" % (label, axis), 32 + i * 150, y + 2, 140, 16, 200, 100, cursor=10)
b.text(p, "tBatt", 4, 220, 472, 24, "battery remote -- robot --", maxl=40)
b.text(p, "tRadio", 4, 244, 472, 24, "radio no link", maxl=44)
# ---------------------------------------------------------------- 3 servos
p = "servos"
# Top view of the robot, front up: right legs on the right.
slots = {5: (4, 36), 4: (4, 114), 3: (4, 192), 0: (304, 36), 1: (304, 114), 2: (304, 192)}
for leg, (x, y) in slots.items():
b.text(p, "tLeg%d" % leg, x, y, 172, 18, LEG_TITLES[leg], pco=LABEL, maxl=12)
for j in range(3):
i = leg * 3 + j
b.text(p, "tJ%d" % i, x, y + 19 + j * 19, 108, 18, ["coxa", "femur", "tibia"][j] + " --", maxl=12)
b.bar(p, "jJ%d" % i, x + 110, y + 22 + j * 19, 62, 13)
b.text(p, "tBody", 196, 40, 88, 200, "FRONT", font=SMALL, pco=TEXT, bco="#303848", xcen="center",
ycen="up", style="border", borderc="#8090a8", borderw=2)
b.text(p, "tSrv", 196, 246, 88, 22, "NO DATA", font=SMALL, pco="#808080", xcen="center", maxl=10)
# ---------------------------------------------------------------- 4 cal
p = "cal"
b.button(p, "bCalOn", 4, 38, 130, 44, "START", [frame("O")], maxl=6)
b.text(p, "tMsg", 140, 38, 336, 44, "Robot on a stand, disarmed: START", font=SMALL, pco="#ffd040", maxl=40, isbr="true")
for i in range(6):
b.button(p, "bL%d" % i, 4 + i * 79, 88, 75, 40, LEG_NAMES[i], [frame("L", i)], maxl=4)
for j, n in enumerate(["COXA", "FEMUR", "TIBIA"]):
b.button(p, "bJ%d" % j, 4 + j * 158, 134, 154, 40, n, [frame("J", j)], maxl=6)
for n, x, t, code in (("bM10", 4, "-10", 0), ("bM1", 78, "-1", 1), ("bP1", 332, "+1", 2), ("bP10", 406, "+10", 3)):
b.button(p, n, x, 180, 70, 44, t, [frame("T", code)], maxl=4)
b.text(p, "tTrim", 152, 180, 176, 44, "FR coxa", font=SMALL, xcen="center", maxl=20, bco="#303848")
b.button(p, "bZeroT", 4, 228, 110, 40, "ZERO", [frame("T", 4)], font=SMALL, maxl=6)
b.button(p, "bRevert", 118, 228, 140, 40, "REVERT", [frame("E")], font=SMALL, maxl=8)
b.button(p, "bSave", 262, 228, 214, 40, "SAVE TO ROBOT", [frame("W")], font=SMALL, maxl=14)
# ---------------------------------------------------------------- 5 setup
p = "setup"
b.text(p, "tHdr1", 4, 36, 472, 22, "STICKS AND HANDSET", pco=LABEL, maxl=20)
b.button(p, "bCal", 4, 60, 170, 44, "CAL STICKS", [frame("C")], font=SMALL, maxl=12)
b.button(p, "bCalX", 180, 60, 120, 44, "CANCEL", [frame("Q")], font=SMALL, maxl=8)
b.button(p, "bZero", 306, 60, 170, 44, "LEVEL REMOTE", [frame("Z")], font=SMALL, maxl=14)
b.text(p, "tHdr2", 4, 110, 472, 22, "DISPLAY", pco=LABEL, maxl=20)
b.button(p, "bUpd", 4, 134, 230, 44, "UPDATE DISPLAY", [frame("U")], font=SMALL, maxl=16)
b.text(p, "tDim", 244, 134, 70, 44, "LIGHT", pco=LABEL)
b.slider(p, "hDim", 314, 142, 162, 28, 100, 100, {"codesslide": ["dim=hDim.val"], "codesup": ["dims=hDim.val"]},
cursor=20)
b.text(p, "tVer", 4, 186, 472, 22, "remote fw --", pco=LABEL, maxl=50)
b.text(p, "tMsg", 4, 212, 472, 54, "", pco="#ffd040", maxl=40, isbr="true")
# ---------------------------------------------------------------- 6 upload
p = "upload"
b.text(p, "t0", 20, 40, 440, 50, "DISPLAY UPDATE MODE", font=BIG, xcen="center", maxl=20)
for i, line in enumerate(("Upload the .tft from Nextion Editor", "over the remote's USB port.", "",
"Power-cycle the remote when done.")):
b.text(p, "t%d" % (i + 1), 20, 110 + i * 26, 440, 24, line, xcen="center", maxl=40)
return path
# Page pre-initialize code after the 'P' frame.
PREINIT = {
# Indicator sliders: no touch, so a finger can't drag them.
"status": ["tsw hRRoll,0", "tsw hRPitch,0", "tsw hBRoll,0", "tsw hBPitch,0",
"tsw hLX,0", "tsw hLY,0", "tsw hLT,0", "tsw hRX,0", "tsw hRY,0", "tsw hRT,0"],
"setup": ["hDim.val=dim"],
}
def main():
ap = argparse.ArgumentParser(description=__doc__.split("\n")[0])
ap.add_argument("--out", default=OUT)
ap.add_argument("--font-dir", help="folder with DejaVuSansMono.ttf and DejaVuSansMono-Bold.ttf")
a = ap.parse_args()
tmp = a.out + ".new"
if os.path.exists(tmp):
os.remove(tmp)
build(tmp, font_dir(a.font_dir))
problems = nx.t_verify(tmp)
if not problems["ok"]:
sys.exit("verify failed: %s" % problems["problems"])
os.replace(tmp, a.out)
print("wrote", a.out)
if __name__ == "__main__":
main()
@@ -2,14 +2,15 @@
#include <Arduino.h> #include <Arduino.h>
// ===================================================================== // =====================================================================
// Remote -> robot wire protocol. SINGLE SOURCE OF TRUTH. // Remote <-> robot wire protocol. SINGLE SOURCE OF TRUTH.
// //
// Both robot/ and remote/ compile against this exact header (PlatformIO // Both robot/ and remote/ compile against this exact header (PlatformIO
// lib_dir = shared). Any change here changes the over-the-air format: // lib_dir = shared). Any change here changes the over-the-air format:
// rebuild and reflash BOTH boards together. The static payload size is set // rebuild and reflash BOTH boards together. Each end drops any packet whose
// to sizeof(CmdPacket) on both radios, so a remote and robot running // length isn't sizeof() of what it expects, so a remote and robot running
// mismatched versions simply won't hear each other (the robot then fails // mismatched versions simply won't hear each other (the robot then fails
// safe on link timeout) instead of misinterpreting fields. // safe on link timeout) instead of misinterpreting fields.
// Telemetry (below) comes back from the robot in the radio's ack payloads.
// //
// Axis convention (right-handed body frame, viewed from above, like ROS // Axis convention (right-handed body frame, viewed from above, like ROS
// REP-103): +X forward, +Y left, +Z up. Positive rotations follow the // REP-103): +X forward, +Y left, +Z up. Positive rotations follow the
@@ -21,6 +22,10 @@
// remote never needs to know robot geometry. // remote never needs to know robot geometry.
// ===================================================================== // =====================================================================
// Protocol version: bump on any change to CmdPacket, Telemetry or the radio
// settings below (the payload sizes are checked on both ends as well).
constexpr uint8_t HXP_PROTOCOL_VERSION = 4;
struct CmdPacket { struct CmdPacket {
int16_t vx = 0; // left stick: + forward / - backward int16_t vx = 0; // left stick: + forward / - backward
int16_t vy = 0; // left stick: + strafe left / - strafe right int16_t vy = 0; // left stick: + strafe left / - strafe right
@@ -29,15 +34,18 @@ struct CmdPacket {
int16_t remote_roll = 0; // remote handset tilt (feedforward), only honored with FLAG_REMOTE_GYRO int16_t remote_roll = 0; // remote handset tilt (feedforward), only honored with FLAG_REMOTE_GYRO
int16_t remote_pitch = 0; int16_t remote_pitch = 0;
int16_t body_height = 0; // standing height setting: -1000 lowest, 0 = default, +1000 highest int16_t body_height = 0; // standing height setting: -1000 lowest, 0 = default, +1000 highest
int16_t cal_value = 0; // servo calibration: centre trim in us for cal_servo (CAL_SET only)
uint8_t speed_scale = 128; // 0-255, multiplies vx/vy uint8_t speed_scale = 128; // 0-255, multiplies vx/vy
uint8_t flags = 0; // bit-packed, see FLAG_* below uint8_t flags = 0; // bit-packed, see FLAG_* below
uint8_t seq = 0; // increments every transmitted packet uint8_t seq = 0; // increments every transmitted packet
uint8_t cal_cmd = 0; // servo calibration command, CAL_* below. 0 = normal operation
uint8_t cal_servo = 0xFF; // servo calibration: selected servo, leg * 3 + joint (0..17)
}; };
// Both ends are Teensy (Cortex-M7, little-endian, 2-byte int16 alignment): // Both ends are Teensy (Cortex-M, little-endian, 2-byte int16 alignment):
// 9 x int16 + 3 x uint8 = 21 bytes, padded to 22. If a different MCU family // 10 x int16 + 5 x uint8 = 25 bytes, padded to 26. If a different MCU family
// ever joins, pack the struct explicitly rather than trusting this. // ever joins, pack the struct explicitly rather than trusting this.
static_assert(sizeof(CmdPacket) == 22, "CmdPacket layout changed - reflash both boards"); static_assert(sizeof(CmdPacket) == 26, "CmdPacket layout changed - reflash both boards");
static_assert(sizeof(CmdPacket) <= 32, "nRF24L01 payloads are limited to 32 bytes"); static_assert(sizeof(CmdPacket) <= 32, "nRF24L01 payloads are limited to 32 bytes");
constexpr uint8_t FLAG_ARMED = 1 << 0; constexpr uint8_t FLAG_ARMED = 1 << 0;
@@ -53,10 +61,63 @@ constexpr uint8_t FLAG_GAIT_WAVE = 1 << 5; // 0 = tripod requested, 1 = wav
constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled
constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled
// ---- Servo centre calibration (CmdPacket.cal_cmd) ----
// Only honoured while FLAG_ARMED is clear. Any CAL_* other than CAL_NONE puts
// the robot in its SERVO_CAL state: every servo is driven to its centre pose
// (coxa 0, femur 0, tibia -90 = legs straight out, shins straight down) through
// the per-servo trims, so a mis-centred horn shows. Every command is a state,
// not an event, so a repeated or lost packet does no harm. Leaving SERVO_CAL
// (CAL_NONE, arming, or link loss) relaxes the servos again.
constexpr uint8_t CAL_NONE = 0; // normal operation
constexpr uint8_t CAL_HOLD = 1; // hold the centre pose; select cal_servo, report its trim
constexpr uint8_t CAL_SET = 2; // ... and set cal_servo's trim to cal_value (us)
constexpr uint8_t CAL_SAVE = 3; // ... and store all trims in the robot's EEPROM (if changed)
constexpr uint8_t CAL_REVERT = 4; // ... and reload all trims from the EEPROM (drop unsaved changes)
constexpr int16_t CAL_TRIM_LIMIT_US = 200; // trims are clamped to +-this
constexpr uint8_t CAL_NO_SERVO = 0xFF;
// =====================================================================
// Robot -> remote telemetry. The robot preloads this as the nRF24 ACK
// PAYLOAD, so it rides back on the auto-ack of the remote's next packet:
// no extra transmissions, and nothing changes for the remote -> robot
// direction. Ack payloads need dynamic payload lengths on both radios, so
// each end checks the received length against sizeof() itself and drops
// anything else - a remote and robot on different protocol versions still
// don't understand each other's packets.
// =====================================================================
struct Telemetry {
int16_t roll_ddeg = 0; // robot IMU roll, 0.1 deg (+ = right side down)
int16_t pitch_ddeg = 0; // robot IMU pitch, 0.1 deg (+ = nose down)
uint16_t battery_mv = 0; // robot battery, mV. 0 = not measured
int16_t cal_trim_us = 0; // centre trim of cal_servo, us
uint8_t state = 0; // TSTATE_* below
uint8_t status = 0; // TSTAT_* bits below
uint8_t cal_servo = CAL_NO_SERVO; // echo of CmdPacket.cal_servo (the trim above belongs to it)
uint8_t rx_per_sec = 0; // remote packets the robot received in the last second
int8_t joint_deg[18] = {}; // commanded joint angles, leg * 3 + joint, degrees.
// Tibia is stored + 90 (its range -150..-20 doesn't fit an int8):
// tibia = joint_deg[i * 3 + 2] - 90.
};
static_assert(sizeof(Telemetry) == 30, "Telemetry layout changed - reflash both boards");
static_assert(sizeof(Telemetry) <= 32, "nRF24L01 ack payloads are limited to 32 bytes");
constexpr int TELEMETRY_TIBIA_OFFSET_DEG = 90;
constexpr uint8_t TSTATE_DISARMED = 0; // same numbering as the robot's RobotState
constexpr uint8_t TSTATE_SIT = 1;
constexpr uint8_t TSTATE_STAND = 2;
constexpr uint8_t TSTATE_WALKING = 3;
constexpr uint8_t TSTATE_SERVO_CAL = 4;
constexpr uint8_t TSTAT_IMU_OK = 1 << 0; // robot IMU responded at power-on
constexpr uint8_t TSTAT_GYRO_ON = 1 << 1; // robot-gyro self-leveling active
constexpr uint8_t TSTAT_SERVOS_ON = 1 << 2; // servos are being driven (not relaxed)
constexpr uint8_t TSTAT_CAL_DIRTY = 1 << 3; // servo trims changed since the last save
constexpr uint8_t TSTAT_TRIMS_STORED = 1 << 4; // the EEPROM holds a trim record
// ---- Radio link settings (must be identical on both ends) ---- // ---- Radio link settings (must be identical on both ends) ----
constexpr uint8_t HXP_RADIO_ADDRESS[6] = "HXPD1"; // 5-byte pipe address + NUL constexpr uint8_t HXP_RADIO_ADDRESS[6] = "HXPD1"; // 5-byte pipe address + NUL
constexpr uint8_t HXP_RADIO_CHANNEL = 96; // 2400+96 MHz; pick one clear of your local Wi-Fi constexpr uint8_t HXP_RADIO_CHANNEL = 96; // 2400+96 MHz; pick one clear of your local Wi-Fi
constexpr uint8_t HXP_RADIO_PAYLOAD = sizeof(CmdPacket); constexpr uint8_t HXP_RADIO_PAYLOAD = sizeof(CmdPacket); // remote -> robot (checked per packet, see Telemetry)
// Data rate / PA level are RF24 enums; kept as plain ints here so this header // Data rate / PA level are RF24 enums; kept as plain ints here so this header
// doesn't depend on RF24.h. 2 == RF24_250KBPS, 1 == RF24_PA_LOW in RF24 1.x; // doesn't depend on RF24.h. 2 == RF24_250KBPS, 1 == RF24_PA_LOW in RF24 1.x;
// both Comms.cpp (robot) and RadioLink.cpp (remote) static_assert this. // both Comms.cpp (robot) and RadioLink.cpp (remote) static_assert this.
+1
View File
@@ -83,6 +83,7 @@ HXSIM_API void hxsim_step(uint32_t dt_us) {
} }
HXSIM_API void hxsim_radio_send(const uint8_t* bytes, int32_t len) { simhw::radioSend(bytes, len); } HXSIM_API void hxsim_radio_send(const uint8_t* bytes, int32_t len) { simhw::radioSend(bytes, len); }
HXSIM_API int32_t hxsim_get_ack_payload(uint8_t* out, int32_t cap) { return simhw::radioLastAckPayload(out, cap); }
HXSIM_API void hxsim_set_imu(float ax, float ay, float az, float gx, float gy, float gz) { HXSIM_API void hxsim_set_imu(float ax, float ay, float az, float gx, float gy, float gz) {
g_manual_imu = true; g_manual_imu = true;
+5 -1
View File
@@ -18,7 +18,7 @@
#endif #endif
// Bump whenever a signature or struct below changes; the C# side checks it. // Bump whenever a signature or struct below changes; the C# side checks it.
#define HXSIM_API_VERSION 7 #define HXSIM_API_VERSION 8
#define HXSIM_NUM_LEGS 6 #define HXSIM_NUM_LEGS 6
@@ -83,6 +83,10 @@ HXSIM_API void hxsim_get_servos(int32_t* us_18, int32_t* attached_18, uint32_
HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded through Config.h's per-servo SERVOS table HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded through Config.h's per-servo SERVOS table
HXSIM_API void hxsim_get_feet_mm(float* xyz_18); // forward kinematics of those angles, body frame HXSIM_API void hxsim_get_feet_mm(float* xyz_18); // forward kinematics of those angles, body frame
HXSIM_API void hxsim_get_status(HxStatus* out); HXSIM_API void hxsim_get_status(HxStatus* out);
// The robot's telemetry (shared/HexapodProtocol Telemetry) as it came back in
// the ack payload of the last packet received. Returns its length in bytes
// (sizeof(Telemetry)), 0 before the first one.
HXSIM_API int32_t hxsim_get_ack_payload(uint8_t* out, int32_t cap);
// Kinematic body (SimBody.cpp): the body rests on the support plane of its // Kinematic body (SimBody.cpp): the body rests on the support plane of its
// feet and grounded feet don't slide. World frame = robot frame at power-on, // feet and grounded feet don't slide. World frame = robot frame at power-on,
// ground at z = 0. Any pointer may be null. // ground at z = 0. Any pointer may be null.
+1 -1
View File
@@ -19,7 +19,7 @@ robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 3.6 firmw
|---|---| |---|---|
| `analogWrite()` hardware PWM (how `ServoDriver` drives the servos) | the pulse width per pin, worked out from duty, resolution and frequency, then decoded back to angles through `Config.h`'s per-servo `SERVOS` table | | `analogWrite()` hardware PWM (how `ServoDriver` drives the servos) | the pulse width per pin, worked out from duty, resolution and frequency, then decoded back to angles through `Config.h`'s per-servo `SERVOS` table |
| `Wire` + MPU6050 registers | a register-level fake MPU6050 at 0x68, fed from the simulated body's tilt and rotation | | `Wire` + MPU6050 registers | a register-level fake MPU6050 at 0x68, fed from the simulated body's tilt and rotation |
| `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match, and the RX FIFO holds 3 | | `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match (any length up to 32 once `enableAckPayload()` turns on dynamic payloads, as on the chip), and the RX FIFO holds 3. `writeAckPayload()` goes back with the next packet received: `hxsim_get_ack_payload` returns it (the robot's `Telemetry`) |
| `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O | | `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O |
`robot_ino.cpp` compiles `robot.ino` itself and exposes read-only views of `robot_ino.cpp` compiles `robot.ino` itself and exposes read-only views of
+26 -1
View File
@@ -213,6 +213,9 @@ struct AirPacket {
static bool g_radio_present = true; static bool g_radio_present = true;
static std::deque<AirPacket> g_air; // packets waiting at the receiver static std::deque<AirPacket> g_air; // packets waiting at the receiver
static constexpr size_t RX_FIFO_DEPTH = 3; static constexpr size_t RX_FIFO_DEPTH = 3;
static std::vector<uint8_t> g_ack_queued; // robot's writeAckPayload(), not sent yet
static bool g_ack_has_queued = false;
static std::vector<uint8_t> g_ack_delivered; // came back with the last packet received
namespace simhw { namespace simhw {
void radioSetPresent(bool present) { g_radio_present = present; } void radioSetPresent(bool present) { g_radio_present = present; }
@@ -225,6 +228,16 @@ void radioSend(const uint8_t* bytes, int len) {
memcpy(p.address, HXP_RADIO_ADDRESS, 5); memcpy(p.address, HXP_RADIO_ADDRESS, 5);
p.payload.assign(bytes, bytes + len); p.payload.assign(bytes, bytes + len);
g_air.push_back(p); g_air.push_back(p);
// The chip acks on reception, carrying whatever ack payload is queued.
if (g_ack_has_queued) {
g_ack_delivered = g_ack_queued;
g_ack_has_queued = false;
}
}
int radioLastAckPayload(uint8_t* out, int cap) {
int n = (int)g_ack_delivered.size() < cap ? (int)g_ack_delivered.size() : cap;
if (n > 0) memcpy(out, g_ack_delivered.data(), n);
return n;
} }
} // namespace simhw } // namespace simhw
@@ -240,7 +253,7 @@ bool RF24::available() {
while (!g_air.empty()) { while (!g_air.empty()) {
const AirPacket& p = g_air.front(); const AirPacket& p = g_air.front();
bool match = p.channel == channel_ && p.rate == data_rate_ && bool match = p.channel == channel_ && p.rate == data_rate_ &&
memcmp(p.address, address_, 5) == 0 && p.payload.size() == payload_size_; memcmp(p.address, address_, 5) == 0 && (dynamic_ || p.payload.size() == payload_size_);
if (match) return true; if (match) return true;
g_air.pop_front(); g_air.pop_front();
} }
@@ -252,3 +265,15 @@ void RF24::read(void* buf, uint8_t len) {
memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size()); memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size());
g_air.pop_front(); g_air.pop_front();
} }
uint8_t RF24::getDynamicPayloadSize() { return g_air.empty() ? 0 : (uint8_t)g_air.front().payload.size(); }
bool RF24::writeAckPayload(uint8_t, const void* buf, uint8_t len) {
if (!dynamic_ || len > 32) return false;
const uint8_t* b = static_cast<const uint8_t*>(buf);
g_ack_queued.assign(b, b + len);
g_ack_has_queued = true;
return true;
}
uint8_t RF24::flush_tx() {
g_ack_has_queued = false;
return 0;
}
+3
View File
@@ -32,5 +32,8 @@ void imuSet(float ax, float ay, float az, float gx, float gy, float gz);
void radioSetPresent(bool present); void radioSetPresent(bool present);
// Puts one packet "on air" using the protocol's channel/address/data rate. // Puts one packet "on air" using the protocol's channel/address/data rate.
void radioSend(const uint8_t* bytes, int len); void radioSend(const uint8_t* bytes, int len);
// The ack payload that came back with the last packet received (what the
// remote would read after write()). Returns its length, 0 if none yet.
int radioLastAckPayload(uint8_t* out, int cap);
} // namespace simhw } // namespace simhw
+1
View File
@@ -37,6 +37,7 @@ inline double degrees(double rad) { return rad * (180.0 / PI); }
void pinMode(uint8_t pin, uint8_t mode); void pinMode(uint8_t pin, uint8_t mode);
void digitalWrite(uint8_t pin, uint8_t value); void digitalWrite(uint8_t pin, uint8_t value);
void analogWrite(uint8_t pin, int value); void analogWrite(uint8_t pin, int value);
inline int analogRead(uint8_t) { return 0; } // nothing analog is simulated
uint32_t analogWriteResolution(uint32_t bits); // returns the previous resolution, like Teensyduino uint32_t analogWriteResolution(uint32_t bits); // returns the previous resolution, like Teensyduino
void analogWriteFrequency(uint8_t pin, float hz); void analogWriteFrequency(uint8_t pin, float hz);
+2 -2
View File
@@ -1,6 +1,6 @@
#pragma once #pragma once
// Not used by robot/ today; here so a future robot-side EEPROM use doesn't // ServoDriver keeps its servo centre trims here. Backed by RAM, starts erased (0xFF),
// break the simulation build silently. Backed by RAM, starts erased (0xFF). // so every simulated power-on starts with no trims stored.
#include <Arduino.h> #include <Arduino.h>
class SimEEPROM { class SimEEPROM {
+11 -2
View File
@@ -8,8 +8,11 @@ enum rf24_datarate_e { RF24_1MBPS = 0, RF24_2MBPS, RF24_250KBPS };
// A simulated radio. Packets the simulation puts "on air" (hxsim_radio_send) // A simulated radio. Packets the simulation puts "on air" (hxsim_radio_send)
// are only received if this radio's channel, data rate, address and static // are only received if this radio's channel, data rate, address and static
// payload size match the sender's, just like the real nRF24L01. The RX FIFO // payload size match the sender's, just like the real nRF24L01 (with dynamic
// holds 3 packets; extra packets are dropped. // payloads - enableAckPayload() - any length up to 32 is received). The RX
// FIFO holds 3 packets; extra packets are dropped. An ack payload queued with
// writeAckPayload() goes back with the next packet received, like the chip's
// auto-ack (hxsim_get_ack_payload shows it).
class RF24 { class RF24 {
public: public:
RF24(uint16_t ce_pin, uint16_t csn_pin) { (void)ce_pin; (void)csn_pin; } RF24(uint16_t ce_pin, uint16_t csn_pin) { (void)ce_pin; (void)csn_pin; }
@@ -27,6 +30,11 @@ public:
bool available(); bool available();
void read(void* buf, uint8_t len); void read(void* buf, uint8_t len);
bool write(const void*, uint8_t) { return false; } // the robot never transmits bool write(const void*, uint8_t) { return false; } // the robot never transmits
void enableDynamicPayloads() { dynamic_ = true; }
void enableAckPayload() { dynamic_ = true; }
uint8_t getDynamicPayloadSize();
bool writeAckPayload(uint8_t pipe, const void* buf, uint8_t len);
uint8_t flush_tx();
private: private:
uint8_t payload_size_ = 32; uint8_t payload_size_ = 32;
@@ -35,4 +43,5 @@ private:
uint8_t address_[5] = {}; uint8_t address_[5] = {};
bool pipe_open_ = false; bool pipe_open_ = false;
bool listening_ = false; bool listening_ = false;
bool dynamic_ = false;
}; };
+77
View File
@@ -227,6 +227,21 @@ int main() {
snprintf(buf, sizeof(buf), "%u servo writes in 1.5 s (0 = every IK solution was rejected as invalid)", writes); snprintf(buf, sizeof(buf), "%u servo writes in 1.5 s (0 = every IK solution was rejected as invalid)", writes);
check(writes > 0, "IK produces valid joint solutions at the stance pose", buf); check(writes > 0, "IK produces valid joint solutions at the stance pose", buf);
printLegs("standing:"); printLegs("standing:");
{
Telemetry t;
int len = hxsim_get_ack_payload(reinterpret_cast<uint8_t*>(&t), sizeof(t));
float deg[18]; hxsim_get_joints_deg(deg);
float worst = 0;
for (int i = 0; i < 18; i++) {
float tj = t.joint_deg[i] - (i % 3 == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0);
worst = fmaxf(worst, fabsf(tj - deg[i]));
}
snprintf(buf, sizeof(buf), "%d bytes, state %u, status %02X, joints off by up to %.1f deg, %u packets/s",
len, t.state, t.status, worst, t.rx_per_sec);
check(len == (int)sizeof(Telemetry) && t.state == TSTATE_STAND && (t.status & TSTAT_SERVOS_ON) &&
worst < 1.5f && t.rx_per_sec >= 90,
"telemetry comes back in the ack payload (state, servo angles, packet rate)", buf);
}
float feet[18]; hxsim_get_feet_mm(feet); float feet[18]; hxsim_get_feet_mm(feet);
float worst = 0; float worst = 0;
@@ -553,6 +568,68 @@ int main() {
s = status(); s = status();
check(!s.link_ok && s.robot_state == 0, "link loss (>0.3 s silence) disarms the robot"); check(!s.link_ok && s.robot_state == 0, "link loss (>0.3 s silence) disarms the robot");
// ---- Servo centre calibration (CAL_* commands, only while disarmed) ----
{
auto tele = []() { Telemetry t; hxsim_get_ack_payload(reinterpret_cast<uint8_t*>(&t), sizeof(t)); return t; };
int us[18], att[18]; uint32_t wr[18];
CmdPacket c; // disarmed
c.cal_cmd = CAL_HOLD;
c.cal_servo = 4; // leg 1 (mid-right) femur
run(c, 0.3f);
float deg[18]; hxsim_get_joints_deg(deg);
hxsim_get_servos(us, att, wr);
float worst = 0; int relaxed = 0;
for (int i = 0; i < 18; i++) {
worst = fmaxf(worst, fabsf(deg[i] - (i % 3 == 2 ? -90.0f : 0.0f)));
if (!att[i]) relaxed++;
}
Telemetry t = tele();
snprintf(buf, sizeof(buf), "state %u, %d relaxed, pose off by up to %.1f deg", t.state, relaxed, worst);
check(t.state == TSTATE_SERVO_CAL && relaxed == 0 && worst < 1.0f,
"CAL_HOLD holds every servo at the centre pose (coxa 0, femur 0, tibia -90)", buf);
int us0 = us[4];
c.cal_cmd = CAL_SET; c.cal_value = 50;
run(c, 0.2f);
hxsim_get_servos(us, att, wr);
t = tele();
snprintf(buf, sizeof(buf), "pulse %d -> %d us, echo servo %u trim %d, status %02X", us0, us[4], t.cal_servo, t.cal_trim_us, t.status);
check(abs(us[4] - us0 - 50) <= 1 && t.cal_servo == 4 && t.cal_trim_us == 50 && (t.status & TSTAT_CAL_DIRTY),
"CAL_SET trims the selected servo and the telemetry echoes it", buf);
c.cal_cmd = CAL_SAVE;
run(c, 0.2f);
t = tele();
check((t.status & TSTAT_TRIMS_STORED) && !(t.status & TSTAT_CAL_DIRTY), "CAL_SAVE stores the trims");
c.cal_cmd = CAL_SET; c.cal_value = -120;
run(c, 0.2f);
c.cal_cmd = CAL_REVERT;
run(c, 0.2f);
t = tele();
snprintf(buf, sizeof(buf), "trim %d after revert", t.cal_trim_us);
check(t.cal_trim_us == 50 && !(t.status & TSTAT_CAL_DIRTY), "CAL_REVERT drops unsaved trims", buf);
c.flags = FLAG_ARMED; // arming wins over calibration
run(c, 0.3f);
t = tele();
check(t.state == TSTATE_SIT, "arming while CAL_* is sent leaves SERVO_CAL (robot arms into SIT)");
c.flags = 0; c.cal_cmd = CAL_SET; c.cal_value = 0; // put the trim back for the checks below
run(c, 0.2f);
c.cal_cmd = CAL_SAVE;
run(c, 0.2f);
c.cal_cmd = CAL_NONE;
run(c, 0.2f);
hxsim_get_servos(us, att, wr);
relaxed = 0;
for (int i = 0; i < 18; i++) if (!att[i]) relaxed++;
t = tele();
snprintf(buf, sizeof(buf), "state %u, %d of 18 relaxed, trim %d", t.state, relaxed, t.cal_trim_us);
check(t.state == TSTATE_DISARMED && relaxed == 18 && t.cal_trim_us == 0, "CAL_NONE relaxes the servos again", buf);
runSilent(0.5f);
}
// ---- Protocol mismatch fails closed ---- // ---- Protocol mismatch fails closed ----
uint8_t short_payload[16] = {}; uint8_t short_payload[16] = {};
short_payload[10] = FLAG_ARMED; short_payload[10] = FLAG_ARMED;