From 91dac16fca27e4426c0f1d2b2bf96aa08f9d49da Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 28 Sep 2026 18:14:00 +0200 Subject: [PATCH] Draft: shared protocol, remote firmware, project docs Not yet verified on hardware. - shared/HexapodProtocol: CmdPacket (now 20 bytes, adds remote_roll / remote_pitch), FLAG_* bits and radio settings as the single protocol definition; static payload size = sizeof(CmdPacket) on both radios. - robot: use the shared protocol; vx = forward (harness w/x <-> q/e); InputMapper adds remote tilt on top of the right stick when FLAG_REMOTE_GYRO is set, and robot.ino no longer zeroes stick roll/pitch when it's off. - remote (Teensy 4.1): Sticks (calibration, EEPROM), RemoteIMU, RemoteState (interlocks, link-loss disarm, smoothing), PacketBuilder, RadioLink, NextionUI, SerialConsole. - CLAUDE.md, README.md, remote/README.md, remote/NEXTION.md. Co-Authored-By: Claude Opus 5.5 --- 2026-current/CLAUDE.md | 174 ++++++++++++++++++ 2026-current/README.md | 32 ++++ 2026-current/remote/Config.h | 110 +++++++++++ 2026-current/remote/NEXTION.md | 75 ++++++++ 2026-current/remote/NextionUI.cpp | 157 ++++++++++++++++ 2026-current/remote/NextionUI.h | 42 +++++ 2026-current/remote/PacketBuilder.cpp | 69 +++++++ 2026-current/remote/PacketBuilder.h | 21 +++ 2026-current/remote/README.md | 112 +++++++++++ 2026-current/remote/RadioLink.cpp | 42 +++++ 2026-current/remote/RadioLink.h | 22 +++ 2026-current/remote/RemoteIMU.cpp | 81 ++++++++ 2026-current/remote/RemoteIMU.h | 28 +++ 2026-current/remote/RemoteState.cpp | 114 ++++++++++++ 2026-current/remote/RemoteState.h | 59 ++++++ 2026-current/remote/SerialConsole.cpp | 30 +++ 2026-current/remote/SerialConsole.h | 14 ++ 2026-current/remote/SmoothedValue.h | 26 +++ 2026-current/remote/Sticks.cpp | 155 ++++++++++++++++ 2026-current/remote/Sticks.h | 47 +++++ 2026-current/remote/UiTypes.h | 45 +++++ 2026-current/remote/remote.ino | 117 ++++++++++++ 2026-current/robot/Comms.cpp | 12 +- 2026-current/robot/Comms.h | 22 +-- 2026-current/robot/Config.h | 3 +- 2026-current/robot/InputMapper.cpp | 10 + 2026-current/robot/README.md | 59 ++++-- 2026-current/robot/robot.ino | 19 +- .../shared/HexapodProtocol/HexapodProtocol.h | 59 ++++++ .../shared/HexapodProtocol/library.json | 7 + 30 files changed, 1706 insertions(+), 57 deletions(-) create mode 100644 2026-current/CLAUDE.md create mode 100644 2026-current/README.md create mode 100644 2026-current/remote/Config.h create mode 100644 2026-current/remote/NEXTION.md create mode 100644 2026-current/remote/NextionUI.cpp create mode 100644 2026-current/remote/NextionUI.h create mode 100644 2026-current/remote/PacketBuilder.cpp create mode 100644 2026-current/remote/PacketBuilder.h create mode 100644 2026-current/remote/README.md create mode 100644 2026-current/remote/RadioLink.cpp create mode 100644 2026-current/remote/RadioLink.h create mode 100644 2026-current/remote/RemoteIMU.cpp create mode 100644 2026-current/remote/RemoteIMU.h create mode 100644 2026-current/remote/RemoteState.cpp create mode 100644 2026-current/remote/RemoteState.h create mode 100644 2026-current/remote/SerialConsole.cpp create mode 100644 2026-current/remote/SerialConsole.h create mode 100644 2026-current/remote/SmoothedValue.h create mode 100644 2026-current/remote/Sticks.cpp create mode 100644 2026-current/remote/Sticks.h create mode 100644 2026-current/remote/UiTypes.h create mode 100644 2026-current/remote/remote.ino create mode 100644 2026-current/shared/HexapodProtocol/HexapodProtocol.h create mode 100644 2026-current/shared/HexapodProtocol/library.json diff --git a/2026-current/CLAUDE.md b/2026-current/CLAUDE.md new file mode 100644 index 0000000..8a44a60 --- /dev/null +++ b/2026-current/CLAUDE.md @@ -0,0 +1,174 @@ +# Hexapod: project memory + +Hexapod walking robot plus a custom handheld remote, both Teensy-based and +linked one-way over nRF24L01 (remote → robot, with hardware auto-ack only). +This file is the persistent context for working on either side. Read it +fully before changing anything. + +## Layout and toolchain + +PlatformIO, one `platformio.ini`, two environments: + +| Env | Board | Sketch dir | Purpose | +|-----|-------|------------|---------| +| `robot` | `teensy40` | `robot/` | walking robot | +| `remote` | `teensy41` | `remote/` | handheld transmitter | + +- `shared/` is PlatformIO's `lib_dir`. `shared/HexapodProtocol/` is a + header-only library both sketches `#include `. +- PlatformIO has a single global `src_dir` and only converts `.ino` files at + its root, so `scripts/select_src_dir.py` (a `pre:` extra script) swaps + `PROJECT_SRC_DIR` to each env's `custom_src_dir`. That keeps `robot/` and + `remote/` plain Arduino sketch folders: one `.ino` plus paired `.h/.cpp` + per concern. **Keep that structure**. No giant files, no `src/` folders. +- RF24 (nrf24/RF24, resolved 1.6.x) comes from `lib_deps`. Servo, Wire, + SPI and EEPROM ship with the Teensy framework. +- Build: `pio run` (both), `pio run -e robot`, `pio run -e remote`. On this + machine pio lives at `C:\Users\patri\.platformio\penv\Scripts\pio.exe`. + Both envs build clean. Build both after touching `shared/`. + +## Communication contract (`shared/HexapodProtocol/HexapodProtocol.h`) + +`CmdPacket`, 20 bytes (static_asserted), sent remote → robot at 100 Hz +(robot `CONTROL_HZ` is 60): + +| Field | Range | Meaning | +|-------|-------|---------| +| `vx` | ±1000 | + forward | +| `vy` | ±1000 | + strafe **left** | +| `yaw_rate` | ±1000 | + turn left (CCW). Ignored by the robot in mode B | +| `rx, ry, rz` | ±1000 | right stick: roll/pitch/yaw, or X/Y/Z offset when `FLAG_RSTICK_XLATE` | +| `remote_roll, remote_pitch` | ±1000 | handset tilt feedforward, used only when `FLAG_REMOTE_GYRO` | +| `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) | +| `seq` | 0–255 | increments per packet (robot doesn't check it yet) | + +- **Axis convention** (ROS REP-103 style): body +X forward, +Y left, +Z up. + +roll = right side down, +pitch = nose down, +yaw = CCW. +- **Normalized fields.** The remote never knows robot geometry. The robot's + `InputMapper` owns the scaling: `MAX_LINEAR_MM_S` 150, + `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 + 96, 250 kbps, PA LOW, **static payload size = sizeof(CmdPacket)** on both + radios. That makes version mismatches fail closed: packets are not + received, the robot times out, and it disarms. +- **Changing the protocol means reflashing both boards.** Keep it ≤ 32 + bytes and update the `static_assert`. +- **Failsafes:** the robot sets `linkOk()` false after `LINK_TIMEOUT_SEC` + (0.3 s) without packets and then uses a default `CmdPacket` (flags 0 = + disarmed, servos detached). The remote disarms itself after + `LINK_LOST_MS` (300 ms) without acks, so the robot can't re-arm itself + when the link returns. +- **Division of labor:** the remote only fills fields per this contract. + Mode A/B interpretation, heading-hold, pose scaling and all smoothing of + pose happen on the robot. Don't duplicate `InputMapper` logic in the + remote. + +## Robot architecture (`robot/`, Teensy 4.0) + +Loop at `CONTROL_HZ` (60): comms → InputMapper → IMU → heading integrate → +StateMachine → gait/sit-stand targets → BodyPose → Limits clamp → IK → +joint-rate clamp → servos. + +| Module | Role | +|--------|------| +| `robot.ino` | setup/loop wiring, open-loop heading estimate, sit/stand blend, `TEST_SERIAL_INPUT` harness | +| `Config.h` | **all placeholder geometry/pins/rates** | +| `SmoothedValue.h` | rate-limited setpoint smoother used for every discrete command | +| `Kinematics` | per-leg 3-DOF analytic IK, `neutralStance()` | +| `GaitEngine` | tripod/wave patterns, per-leg phase, pattern swap only at touchdown | +| `BodyPose` | 6-axis body pose (smoothed command + IMU roll/pitch correction) applied to foot targets | +| `RobotIMU` | MPU6050 complementary filter (α 0.98), gated by `FLAG_ROBOT_GYRO` | +| `StateMachine` | DISARMED → SIT → STAND → WALKING, sit/stand blend at 1/s. Disarm is instant | +| `InputMapper` | CmdPacket → Twist + PoseCommand + mode flags. Mode B = heading-hold from stick angle. 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) | +| `Comms` | nRF24 RX, link timeout, `injectPacket()` for the harness | +| `ServoDriver` | attach/detach/write, skips invalid IK solutions | + +## Remote architecture (`remote/`, Teensy 4.1) + +Loop: drain Nextion + USB serial input every pass. On each `TX_HZ` (100) +tick: sticks → IMU → radio link status → RemoteState (smoothers, +link-loss disarm) → PacketBuilder → `radio.write` → Nextion render. + +| Module | Role | +|--------|------| +| `remote.ino` | loop wiring, status print, packet dump | +| `Config.h` | **all placeholder pins, calibration defaults, deadzones, rates** | +| `UiTypes.h` | `UiCmd`/`UiEvent` (operator requests), `UiView` (what the display shows) | +| `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 | +| `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 | +| `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 % | +| `NextionUI` | library-free Nextion driver. HMI sends `'#' len cmd [val]` frames via `printh`. Remote pushes state by component name with diffing, a 2 s full refresh, and a redraw on the Nextion 0x88 "ready" frame | +| `SerialConsole` | USB keystrokes that mirror every UI command plus diagnostics | +| `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 | + +Remote stick-frame convention: logical X + = right, Y + = forward, twist + += CCW. The mapping into the packet is in `PacketBuilder.cpp` (e.g. +`vy = -L_X`, translate mode `ry = -R_X`). + +## Testing without the other board + +- **Robot without remote:** `#define TEST_SERIAL_INPUT` in `robot.ino` (on + by default). Serial 115200, single keys: `a` arm, `s` sit/stand, `w/x` + vx ±200, `q/e` vy ±200 (left/right), `z/c` yaw_rate ∓/±200, `g` gait, + `m` mode, space = zero velocity. Keys inject through + `Comms::injectPacket()`, the same path radio packets take. Order: arm → + stand → walk. The harness does not exercise the right stick or remote + tilt. Add keys if needed. +- **Remote without robot or display:** the USB serial console (see + `remote/README.md`). `p` shows raw/mapped sticks, IMU, link and state. + `d` dumps packets at 10 Hz. With no robot, the link shows DOWN and + arming still works; the link-loss disarm only fires after the link was + up once. + +## Placeholders and bench-check items (not yet verified on hardware) + +Robot (`robot/README.md` has detail): +- All of `Config.h`: leg mount angles/positions, link lengths, joint + limits, stance radius/height, servo pins, µs/deg calibration, + `MAX_JOINT_DEG_S` (in robot.ino), Limits envelope, InputMapper maxima. +- **Pin conflict:** `SERVO_PINS` leg 3 uses pins 11 and 12, which are + Teensy 4.0 SPI MOSI/MISO needed by the nRF24. Must be moved. Some servo + pins (26, 27, 30…) may also not be PWM-capable on the 4.0. Verify. +- IK knee direction (`tibia_rad` sign), BodyPose roll/pitch sign, IMU + register map, and the sit pose assuming feet stay grounded. +- **Suspected gait direction inversion:** in `GaitEngine::update` the + stance phase moves the foot from `neutral - half` to `neutral + half`, + i.e. with the commanded direction. That pushes the body *backward* for + +vx, and the same happens for yaw. Fix = swap start/end. Flagged to the + owner, **not changed**. The harness `z`/`c` yaw keys currently rely on + this behavior and need rechecking after the fix. +- Heading for mode B is open-loop (integrated commanded ω). Fusing IMU yaw + is a planned upgrade. +- `Limits::stancePolygonOk()` is computed but not acted on. + +Remote (`remote/README.md` has detail): +- All pins in `remote/Config.h` (radio CE 9 / CSN 10, sticks A0–A3/A6/A7, + MPU6050 on 18/19, Nextion on Serial2 7/8). +- `STICK_INVERT[]`, calibration defaults, deadzones, expo. +- `IMU_AXIS_FROM_CHIP` / `IMU_AXIS_SIGN` depend on how the MPU6050 is + mounted in the handset. +- The Nextion must have `bauds=115200` in Program.s, and the HMI must match + `NEXTION.md`. No `.HMI` file is in the repo yet. The compiled `.tft` is + gitignored. +- Radio retry timing (`RADIO_RETRY_*`) is untested under real link loss. + +## Working rules for this repo + +- The **robot side is owner-verified code**. Don't edit `robot/` unless + something is factually wrong relative to the protocol, and ask the owner + first even then. Flag suspected robot bugs instead of fixing them. +- Protocol changes go in `shared/HexapodProtocol/` only. Update the size + `static_assert`, this file's contract table, and both READMEs. +- Match the existing style: config constants at the top of `Config.h` + with BENCH comments for anything hardware-dependent, `SmoothedValue` + rate limiting on any setpoint that can jump, and paired `.h/.cpp` per + concern. +- Planned or possible next steps: an ack-payload telemetry channel from + robot → remote (state, battery, link) to show real robot state on the + Nextion, battery monitoring on both sides, a `.HMI` file checked into + `remote/`. diff --git a/2026-current/README.md b/2026-current/README.md new file mode 100644 index 0000000..1e61ee3 --- /dev/null +++ b/2026-current/README.md @@ -0,0 +1,32 @@ +# Hexapod + +A six-legged walking robot (Teensy 4.0) and its handheld remote +(Teensy 4.1 + Nextion touchscreen), linked over nRF24L01. + +``` +robot/ robot firmware: IK, gait, body pose, IMU stabilization, state machine +remote/ remote firmware: 2 x 3-axis sticks, MPU6050 tilt, Nextion UI, RF24 TX +shared/ HexapodProtocol: the CmdPacket wire format + radio settings used by both +scripts/ PlatformIO helper (per-environment sketch folder) +``` + +## Build (PlatformIO) + +``` +pio run # build both +pio run -e robot -t upload +pio run -e remote -t upload +pio device monitor -e robot +``` + +## Where to go next + +- **[robot/README.md](robot/README.md)**: placeholder geometry to measure, + the bench-check checklist, and the serial test harness for driving the + robot without a remote. +- **[remote/README.md](remote/README.md)**: wiring, control scheme, stick + calibration, and the remote's serial console. +- **[remote/NEXTION.md](remote/NEXTION.md)**: how to build the touchscreen + HMI. +- **[shared/HexapodProtocol/HexapodProtocol.h](shared/HexapodProtocol/HexapodProtocol.h)**: + the protocol. Change it only by reflashing both boards. diff --git a/2026-current/remote/Config.h b/2026-current/remote/Config.h new file mode 100644 index 0000000..d03f44e --- /dev/null +++ b/2026-current/remote/Config.h @@ -0,0 +1,110 @@ +#pragma once +#include + +// ===================================================================== +// Remote handset config (Teensy 4.1). Everything here is a placeholder +// until checked against your real wiring and parts - see the BENCH notes. +// Radio address/channel/rate/PA/payload size are NOT here: they live in +// shared/HexapodProtocol so they can't drift from the robot. +// ===================================================================== + +// ---- Loop rate ---- +// Must be >= the robot's CONTROL_HZ (60) so every robot control tick sees a +// fresh packet. 100 Hz gives margin for the odd dropped packet. +constexpr float TX_HZ = 100.0f; + +// ---- Radio (nRF24L01) on SPI0: MOSI 11, MISO 12, SCK 13 ---- +// BENCH: placeholder CE/CSN pins - match your wiring. +constexpr uint8_t RADIO_CE_PIN = 9; +constexpr uint8_t RADIO_CSN_PIN = 10; +// Auto-retransmit: (delay units of 250us, count). Kept short on purpose - a +// newer packet replaces a lost one 10 ms later anyway, and write() blocks for +// the whole retry sequence. Worst case here is ~4 attempts x ~1.7 ms. +constexpr uint8_t RADIO_RETRY_DELAY = 1; +constexpr uint8_t RADIO_RETRY_COUNT = 3; +// No acks for this long -> remote considers the link lost and disarms itself, +// mirroring the robot's LINK_TIMEOUT_SEC (0.3 s) failsafe, so the robot can't +// spring back up on its own when the link returns. +constexpr uint32_t LINK_LOST_MS = 300; + +// ---- Joysticks: 2 x 3-axis analog potentiometer sticks ---- +// Logical axis order used everywhere in the remote code. +enum StickAxis : uint8_t { L_X = 0, L_Y, L_TWIST, R_X, R_Y, R_TWIST, NUM_AXES }; + +// BENCH: placeholder pins. A4/A5 (18/19) are skipped because they're the I2C +// pins the MPU6050 uses. +constexpr uint8_t STICK_PINS[NUM_AXES] = { A0, A1, A2, A3, A6, A7 }; + +// Each logical axis must end up: X + = pushed RIGHT, Y + = pushed FORWARD +// (away from you), TWIST + = rotated COUNTERCLOCKWISE seen from above. +// BENCH: flip any axis that reads the wrong way on the serial 'p' status dump. +constexpr bool STICK_INVERT[NUM_AXES] = { false, false, false, false, false, false }; + +constexpr int ADC_BITS = 12; // 0..4095 +constexpr int ADC_AVERAGING = 8; // hardware averaging per analogRead() + +// Fallback calibration used until you run the on-device sweep calibration +// (which is saved to EEPROM and overrides these). +// BENCH: typical pot sticks don't use the full electrical range - run the +// sweep calibration once, then optionally paste the printed values here. +constexpr int16_t STICK_DEFAULT_MIN = 200; +constexpr int16_t STICK_DEFAULT_CENTER = 2048; +constexpr int16_t STICK_DEFAULT_MAX = 3900; + +// At power-on the resting position of each axis is re-captured as its center +// (pots drift a little with temperature). Rejected if a stick is visibly +// deflected, i.e. further than this from the stored center (ADC counts). +constexpr int16_t CENTER_CAPTURE_TOLERANCE = 250; +// A sweep calibration is rejected unless every axis travels at least this +// far on both sides of center (ADC counts) - catches "forgot to move a stick". +constexpr int16_t CAL_MIN_HALF_SPAN = 600; + +// Deadzone as a fraction of full deflection; output is rescaled so motion +// starts smoothly from 0 at the deadzone edge instead of jumping. +constexpr float STICK_DEADZONE[NUM_AXES] = { 0.06f, 0.06f, 0.10f, 0.06f, 0.06f, 0.10f }; +// 0 = linear, 1 = fully cubic. A bit of expo gives fine control near center. +constexpr float STICK_EXPO[NUM_AXES] = { 0.25f, 0.25f, 0.30f, 0.20f, 0.20f, 0.20f }; +// One-pole low-pass on raw ADC counts (1 = no filtering). Pots are noisy. +constexpr float STICK_FILTER_ALPHA = 0.5f; + +// ---- Remote IMU: MPU6050 on I2C (Wire: SDA 18, SCL 19) ---- +constexpr uint8_t IMU_ADDR = 0x68; // 0x69 if AD0 is pulled high +// Mounting remap: which chip axis (0=X,1=Y,2=Z) and sign gives each handset +// axis. Handset frame: +X = away from you, +Y = your left, +Z = up. +// BENCH: with the defaults the chip is assumed flat, component side up, its +// X arrow pointing away from you. Verify with the serial 'p' dump: tipping +// the far edge down must read +pitch, dipping the right side must read +roll. +constexpr uint8_t IMU_AXIS_FROM_CHIP[3] = { 0, 1, 2 }; +constexpr int8_t IMU_AXIS_SIGN[3] = { +1, +1, +1 }; +constexpr float IMU_ALPHA = 0.98f; // complementary filter, same as the robot +constexpr int IMU_GYRO_BIAS_SAMPLES = 200; // at boot; hold the remote still + +// Handset tilt -> CmdPacket.remote_roll/pitch. This many degrees of tilt away +// from the zeroed "level" pose = full scale (1000); the robot then scales +// full scale to its own MAX_TILT_DEG. +constexpr float REMOTE_TILT_FULL_SCALE_DEG = 30.0f; +constexpr float REMOTE_TILT_DEADZONE_DEG = 2.0f; // ignore hand tremor around level +// Toggling "remote gyro" fades the tilt in/out at this rate (fraction/s) +// instead of stepping it. The robot rate-limits pose as well; this just +// keeps the flag change itself from being a hard edge. +constexpr float REMOTE_TILT_FADE_PER_SEC = 2.0f; + +// ---- Operator setpoints ---- +constexpr uint8_t SPEED_SCALE_DEFAULT = 128; +constexpr float SPEED_SCALE_RATE_PER_SEC = 255.0f; // slider jumps ramp over <= 1 s (robot doesn't smooth twist) +constexpr uint8_t SPEED_SCALE_STEP = 25; // serial console +/- step +// FLAG_WALK stays set this long after the left stick returns to center, so +// hovering at the deadzone edge doesn't chatter STAND <-> WALKING. +constexpr uint32_t WALK_LINGER_MS = 250; +// Arming is refused unless every stick axis is within this of center +// (post-deadzone, -1000..1000 scale), so the robot never arms mid-command. +constexpr int16_t ARM_CENTER_TOLERANCE = 50; + +// ---- Nextion touchscreen on Serial2 (RX2 = pin 7, TX2 = pin 8) ---- +#define NEXTION_SERIAL Serial2 +// BENCH: Nextion ships at 9600. The HMI's Program.s must set `bauds=115200` +// (see remote/NEXTION.md) or change this to match. +constexpr uint32_t NEXTION_BAUD = 115200; +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 +constexpr uint32_t STATUS_MESSAGE_MS = 2500; // how long transient status messages stay up diff --git a/2026-current/remote/NEXTION.md b/2026-current/remote/NEXTION.md new file mode 100644 index 0000000..d3b8bde --- /dev/null +++ b/2026-current/remote/NEXTION.md @@ -0,0 +1,75 @@ +# Nextion HMI build guide + +The firmware drives the display by **component name**, and the display +talks back with small custom frames sent from each button's touch event. +Build one page in Nextion Editor to match the tables below. Layout, fonts +and colors are up to you. The names and event code must match exactly. + +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 +``` + +This sets the display to the `NEXTION_BAUD` in `remote/Config.h`. The +display ships at 9600, and `bauds=` persists across power cycles. + +Name the page `main` (page 0). The firmware never changes pages. + +## Components + +Text components need **txt_maxl** at least as large as the longest string +the firmware writes. Button bounds here allow some slack. + +| Name | Type | txt_maxl | Firmware writes | +|----------|---------|----------|----------------------------------------------| +| `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 %) | +| `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 + +Put this in each component's **Touch Release Event** (the Speed slider +uses **Touch Move** as well so the value tracks your finger). Frame format: +`'#'` (0x23), length, command byte, optional value byte. + +| Component | Event code | Command | +|-----------|------------------------------------------|--------------------------------| +| `bArm` | `printh 23 01 41` | `A` toggle arm (needs sticks centered) | +| `bKill` | `printh 23 01 4B` | `K` disarm, always accepted | +| `bStand` | `printh 23 01 53` | `S` toggle sit/stand | +| `bGait` | `printh 23 01 47` | `G` tripod ↔ wave | +| `bMode` | `printh 23 01 4D` | `M` direct ↔ heading-hold | +| `bRs` | `printh 23 01 58` | `X` right stick rotate ↔ translate | +| `bRGyro` | `printh 23 01 52` | `R` remote-gyro tilt on/off | +| `bBGyro` | `printh 23 01 42` | `B` robot self-stabilize on/off | +| `hSpeed` | `printh 23 02 56`
`prints hSpeed.val,1` | `V` + speed byte | +| `bZero` | `printh 23 01 5A` | `Z` zero handset level | +| `bCal` | `printh 23 01 43` | `C` start / save stick calibration | +| `bCalX` | `printh 23 01 51` | `Q` cancel calibration | + +**Uncheck "Send Component ID"** on every component. The firmware ignores +standard touch frames, and they only add serial traffic. + +Protocol details and the parser live in `NextionUI.h/.cpp`. To add a +control, pick an unused command byte, add it to `NextionUI::decodeCustom()` +and `UiCmd`, handle it in `RemoteState::handle()`, and add a row here. diff --git a/2026-current/remote/NextionUI.cpp b/2026-current/remote/NextionUI.cpp new file mode 100644 index 0000000..1e83552 --- /dev/null +++ b/2026-current/remote/NextionUI.cpp @@ -0,0 +1,157 @@ +#include "NextionUI.h" +#include + +// RGB565 colors as Nextion expects them. +static constexpr uint16_t COL_ON = 2016; // green +static constexpr uint16_t COL_OFF = 50712; // Nextion default light gray +static constexpr uint16_t COL_DANGER = 63488; // red +static constexpr uint16_t COL_WARN = 64512; // orange + +// Serial2's default 64-byte TX buffer would make a full redraw (~300 bytes +// at 115200 baud) block the TX loop for ~25 ms. Give it room to queue. +static uint8_t tx_extra[1024]; +static uint8_t rx_extra[128]; + +void NextionUI::begin() { + NEXTION_SERIAL.begin(NEXTION_BAUD); + NEXTION_SERIAL.addMemoryForWrite(tx_extra, sizeof(tx_extra)); + NEXTION_SERIAL.addMemoryForRead(rx_extra, sizeof(rx_extra)); + cmd("bkcmd=0"); // no success/fail replies - we never need them + force_full_ = true; +} + +void NextionUI::cmd(const char* fmt, ...) { + char buf[64]; + va_list ap; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + NEXTION_SERIAL.print(buf); + NEXTION_SERIAL.write(0xFF); + NEXTION_SERIAL.write(0xFF); + NEXTION_SERIAL.write(0xFF); +} + +// ---- Receive ---- + +bool NextionUI::decodeCustom(UiEvent& out) { + out.value = body_len_ > 1 ? body_[1] : 0; + switch (body_[0]) { + case 'A': out.cmd = UiCmd::ToggleArm; break; + case 'K': out.cmd = UiCmd::Disarm; break; + case 'S': out.cmd = UiCmd::ToggleStand; break; + case 'G': out.cmd = UiCmd::ToggleGait; break; + case 'M': out.cmd = UiCmd::ToggleWalkMode; break; + case 'X': out.cmd = UiCmd::ToggleRStickXlate; break; + case 'R': out.cmd = UiCmd::ToggleRemoteGyro; break; + case 'B': out.cmd = UiCmd::ToggleRobotGyro; break; + case 'V': out.cmd = UiCmd::SetSpeed; break; + case 'Z': out.cmd = UiCmd::ZeroGyro; break; + case 'C': out.cmd = UiCmd::CalStartSave; break; + case 'Q': out.cmd = UiCmd::CalCancel; break; + default: return false; + } + return true; +} + +bool NextionUI::poll(UiEvent& out) { + while (NEXTION_SERIAL.available()) { + uint8_t b = NEXTION_SERIAL.read(); + switch (rx_) { + case Rx::Idle: + if (b == '#') { + rx_ = Rx::CustomLen; + } else { + rx_ = Rx::Native; + native_first_ = b; + native_len_ = 1; + ff_run_ = (b == 0xFF) ? 1 : 0; + } + break; + + case Rx::CustomLen: + if (b >= 1 && b <= sizeof(body_)) { + body_len_ = b; + body_got_ = 0; + rx_ = Rx::CustomBody; + } else { + rx_ = Rx::Idle; // garbage - resync on the next '#' + } + break; + + case Rx::CustomBody: + body_[body_got_++] = b; + if (body_got_ == body_len_) { + rx_ = Rx::Idle; + if (decodeCustom(out)) return true; + } + break; + + case Rx::Native: + native_len_++; + ff_run_ = (b == 0xFF) ? ff_run_ + 1 : 0; + if (ff_run_ == 3) { + // 0x88 = "Nextion ready" after power-on/reset, 0x00 = startup. + if (native_first_ == 0x88 || native_first_ == 0x00) force_full_ = true; + rx_ = Rx::Idle; + } else if (native_len_ > 16) { + rx_ = Rx::Idle; // not a frame we understand + } + break; + } + } + return false; +} + +// ---- Render ---- + +void NextionUI::pushState(const UiView& v, bool full) { + const UiView& s = shown_; + if (full || v.armed != s.armed) { + cmd("bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED"); + cmd("bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF); + } + if (full || v.stand != s.stand) { + cmd("bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT"); + cmd("bStand.bco=%u", v.stand ? COL_ON : COL_OFF); + } + if (full || v.gait_wave != s.gait_wave) cmd("bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD"); + if (full || v.mode_b != s.mode_b) cmd("bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT"); + if (full || v.rstick_xlate != s.rstick_xlate) cmd("bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE"); + if (full || v.remote_gyro != s.remote_gyro) cmd("bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF); + if (full || v.robot_gyro != s.robot_gyro) cmd("bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF); + if (full || v.speed_target != s.speed_target) { + cmd("hSpeed.val=%u", v.speed_target); + cmd("nSpeed.val=%u", (v.speed_target * 100 + 127) / 255); + } + if (full || v.calibrating != s.calibrating) { + cmd("bCal.txt=\"%s\"", v.calibrating ? "SAVE CAL" : "CAL"); + cmd("bCal.bco=%u", v.calibrating ? COL_WARN : COL_OFF); + } + if (full || v.message != s.message) cmd("tMsg.txt=\"%s\"", v.message); +} + +void NextionUI::pushLink(const UiView& v) { + if (v.link_ok) cmd("tLink.txt=\"LINK %u%%\"", v.link_pct); + else cmd("tLink.txt=\"NO LINK\""); + cmd("tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER); +} + +void NextionUI::render(const UiView& v, uint32_t now_ms) { + bool full = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS; + if (full) { + force_full_ = false; + last_full_ms_ = now_ms; + } + + pushState(v, full); + + // Link quality changes nearly every packet; repaint it at a readable rate, + // but immediately when the link comes or goes. + if (full || v.link_ok != shown_.link_ok || (now_ms - last_link_ms_) >= NEXTION_LINK_REFRESH_MS) { + pushLink(v); + last_link_ms_ = now_ms; + } + + shown_ = v; +} diff --git a/2026-current/remote/NextionUI.h b/2026-current/remote/NextionUI.h new file mode 100644 index 0000000..5cb55ef --- /dev/null +++ b/2026-current/remote/NextionUI.h @@ -0,0 +1,42 @@ +#pragma once +#include "UiTypes.h" +#include "Config.h" + +// Nextion serial HMI driver. No Nextion library - the protocol is small: +// +// Nextion -> remote: custom frames the HMI's touch events send with printh: +// '#' [value] e.g. printh 23 01 41 (ToggleArm) +// printh 23 02 56 + prints hSpeed.val,1 +// Command bytes are listed in NEXTION.md and in NextionUI.cpp. +// Native Nextion return frames (terminated FF FF FF) are also parsed so a +// display reset (0x88 "ready") triggers a full redraw. +// +// remote -> Nextion: plain instructions ("bArm.txt=\"ARMED\"" + FF FF FF). +// +// The remote is authoritative: buttons are plain buttons (not dual-state), +// they only send 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 { +public: + void begin(); + bool poll(UiEvent& out); // call every loop; returns one decoded event at a time + void render(const UiView& v, uint32_t now_ms); + +private: + enum class Rx : uint8_t { Idle, CustomLen, CustomBody, Native }; + Rx rx_ = Rx::Idle; + uint8_t body_[4]; + uint8_t body_len_ = 0, body_got_ = 0; + uint8_t native_first_ = 0, native_len_ = 0, ff_run_ = 0; + + UiView shown_; + bool force_full_ = true; + uint32_t last_full_ms_ = 0; + uint32_t last_link_ms_ = 0; + + void cmd(const char* fmt, ...); + void pushState(const UiView& v, bool full); + void pushLink(const UiView& v); + bool decodeCustom(UiEvent& out); +}; diff --git a/2026-current/remote/PacketBuilder.cpp b/2026-current/remote/PacketBuilder.cpp new file mode 100644 index 0000000..6217776 --- /dev/null +++ b/2026-current/remote/PacketBuilder.cpp @@ -0,0 +1,69 @@ +#include "PacketBuilder.h" + +// Handset tilt (deg) -> -1000..1000 with a small deadzone for hand tremor, +// scaled by the fade blend so toggling remote gyro ramps instead of steps. +int16_t PacketBuilder::tiltToWire(float deg, float blend) { + float mag = fabsf(deg) - REMOTE_TILT_DEADZONE_DEG; + if (mag <= 0 || blend <= 0) return 0; + float n = constrain(mag / (REMOTE_TILT_FULL_SCALE_DEG - REMOTE_TILT_DEADZONE_DEG), 0.0f, 1.0f); + return (int16_t)lroundf(copysignf(n, deg) * 1000.0f * blend); +} + +CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms) { + CmdPacket p; + p.seq = seq_++; + p.speed_scale = st.speedCurrent(); + + uint8_t f = 0; + if (st.armed()) f |= FLAG_ARMED; + if (st.standRequested()) f |= FLAG_STAND; + if (st.modeB()) f |= FLAG_MODE_B; + if (st.rstickXlate()) f |= FLAG_RSTICK_XLATE; + if (st.gaitWave()) f |= FLAG_GAIT_WAVE; + if (st.robotGyro()) f |= FLAG_ROBOT_GYRO; + + // Disarmed: send a neutral command. The robot ignores motion then anyway, + // but this keeps its pose smoothers from winding up to a stale stick. + if (!st.armed()) { + p.flags = f; + return p; + } + + // ---- Left stick: walking. Handset stick frame -> body frame: + // stick forward (+Y) -> +vx forward, stick right (+X) -> -vy (vy + = left), + // twist CCW -> +yaw_rate (turn left). + p.vx = s.v[L_Y]; + p.vy = -s.v[L_X]; + p.yaw_rate = s.v[L_TWIST]; + + // ---- Right stick: body pose. Each packet field is "about / along" one body + // axis, so the physical-axis mapping differs per sub-mode: + if (st.rstickXlate()) { + p.rx = s.v[R_Y]; // X offset: stick forward -> body forward + p.ry = -s.v[R_X]; // Y offset: stick right -> body right (-Y) + p.rz = s.v[R_TWIST]; // Z offset: twist CCW -> body up + } else { + p.rx = s.v[R_X]; // roll: stick right -> right side down + p.ry = s.v[R_Y]; // pitch: stick forward -> nose down + p.rz = s.v[R_TWIST]; // yaw: twist CCW -> body yaws left + } + + // ---- Walk flag: in mode B the robot ignores yaw_rate, so twist alone + // shouldn't start stepping. Linger avoids STAND<->WALKING chatter. + bool walk_input = p.vx != 0 || p.vy != 0 || (!st.modeB() && p.yaw_rate != 0); + if (walk_input) { walk_seen_ = true; last_walk_input_ms_ = now_ms; } + bool walking = walk_seen_ && (now_ms - last_walk_input_ms_) < WALK_LINGER_MS; + if (st.standRequested() && (walk_input || walking)) f |= FLAG_WALK; + + // ---- Remote tilt feedforward: keep the flag up for the whole fade-out so + // the robot sees the tilt ramp to zero rather than being cut off. + float blend = st.remoteTiltBlend(); + if (st.remoteGyro() || blend > 0.0f) { + f |= FLAG_REMOTE_GYRO; + p.remote_roll = tiltToWire(imu.rollDeg(), blend); + p.remote_pitch = tiltToWire(imu.pitchDeg(), blend); + } + + p.flags = f; + return p; +} diff --git a/2026-current/remote/PacketBuilder.h b/2026-current/remote/PacketBuilder.h new file mode 100644 index 0000000..3628481 --- /dev/null +++ b/2026-current/remote/PacketBuilder.h @@ -0,0 +1,21 @@ +#pragma once +#include +#include "Sticks.h" +#include "RemoteIMU.h" +#include "RemoteState.h" + +// The inverse of the robot's InputMapper: turns physical stick axes, handset +// tilt and operator state into a CmdPacket. It only fills fields per the +// protocol contract (HexapodProtocol.h); all physical scaling and mode +// interpretation stays on the robot. +class PacketBuilder { +public: + CmdPacket build(const StickFrame& s, const RemoteIMU& imu, const RemoteState& st, uint32_t now_ms); + +private: + uint8_t seq_ = 0; + bool walk_seen_ = false; + uint32_t last_walk_input_ms_ = 0; + + static int16_t tiltToWire(float deg, float blend); +}; diff --git a/2026-current/remote/README.md b/2026-current/remote/README.md new file mode 100644 index 0000000..c25f482 --- /dev/null +++ b/2026-current/remote/README.md @@ -0,0 +1,112 @@ +# Hexapod remote firmware (Teensy 4.1) + +Handheld transmitter for the hexapod. It reads two 3-axis twist-grip +joysticks and an MPU6050, takes operator commands from a Nextion +touchscreen, and sends a `CmdPacket` (defined in +`../shared/HexapodProtocol/HexapodProtocol.h`) to the robot over nRF24L01 +at 100 Hz. + +## Building + +From the repo root: `pio run -e remote`, flash with +`pio run -e remote -t upload`, monitor with `pio device monitor -e remote`. + +## Wiring (placeholders; edit `Config.h` to match) + +| Part | Signal | Teensy 4.1 pin | +|------|--------|----------------| +| nRF24L01 | MOSI / MISO / SCK | 11 / 12 / 13 (SPI0) | +| nRF24L01 | CE / CSN | 9 / 10 | +| nRF24L01 | VCC | 3.3 V, with a 10–100 µF cap right at the module | +| MPU6050 | SDA / SCL | 18 / 19 (Wire) | +| Nextion | TX → RX2, RX ← TX2 | 7 / 8 (Serial2), 5 V supply | +| Left stick | X / Y / twist | A0 / A1 / A2 | +| Right stick | X / Y / twist | A3 / A6 / A7 | + +Stick pots are wired 3.3 V – wiper – GND. **Never feed 5 V into a Teensy +4.x analog pin.** + +## Control scheme + +| Input | Packet field | Meaning on the robot | +|-------|--------------|----------------------| +| Left stick forward/back | `vx` | walk forward/back | +| Left stick left/right | `vy` | strafe (mode A); heading target direction (mode B) | +| Left stick twist | `yaw_rate` | turn rate (mode A only; ignored in mode B) | +| Right stick, **ROTATE** (default) | `rx/ry/rz` | body roll / pitch / yaw | +| Right stick, **MOVE** (`FLAG_RSTICK_XLATE`) | `rx/ry/rz` | body X / Y / Z (height, via twist) offset | +| Handset tilt | `remote_roll/pitch` | extra body roll/pitch while REMOTE GYRO is on | + +Touchscreen: ARM / DISARM, SIT/STAND, TRIPOD/WAVE, DIRECT/HEADING, +R: ROTATE/MOVE, REMOTE GYRO, ROBOT GYRO, speed slider, LEVEL (re-zero the +handset tilt), CAL / CANCEL CAL. `NEXTION.md` has the HMI build guide. + +### Safety interlocks (in `RemoteState`) + +- **Arming** is refused unless every stick axis is centered, and never + allowed during calibration. +- **Disarming** always clears the stand request. The robot arms into SIT, + so re-arming never stands it up by itself. +- **Link loss:** if no ack arrives for `LINK_LOST_MS` (300 ms, matching the + robot's failsafe), the remote disarms itself. When the link comes back + the robot stays down until you re-arm deliberately. +- **Sit/stand** requests are only accepted while armed. `FLAG_WALK` is only + sent while standing is requested. +- While disarmed or calibrating, all motion fields are sent as zero. + +### Smoothing + +- **Speed scale** ramps toward the slider at `SPEED_SCALE_RATE_PER_SEC`. + The robot uses speed scale directly on the walking twist, so a slider + jump would otherwise be a velocity step. +- **Remote tilt** fades in/out over ~0.5 s when REMOTE GYRO is toggled, and + the handset re-zeroes its "level" at the moment you enable it. +- **FLAG_WALK** lingers 250 ms after the left stick centers, so hovering at + the deadzone edge doesn't make the robot chatter between STAND and + WALKING. +- Body pose smoothing is left to the robot's `BodyPose` rate limiters. + +## Stick calibration + +1. Power on with both sticks released and the remote held still. Each + axis's resting center is captured automatically. +2. For full calibration, disarm, then tap **CAL** (or press `c` on the + serial console). Move every axis of both sticks to both limits, release + them, and tap **SAVE CAL** (`c`). The result is saved to EEPROM. +3. If any axis moved less than `CAL_MIN_HALF_SPAN` either side of center, + the save is refused and you stay in calibration mode. **CANCEL CAL** + (`x`) aborts. +4. `p` prints the calibration in a form you can paste into `Config.h` + defaults. `R` wipes EEPROM back to the `Config.h` defaults. + +## Serial console (USB, 115200, single keystrokes) + +Every touchscreen control is available here, so you can bench-test with no +display attached. + +| Key | Action | Key | Action | +|-----|--------|-----|--------| +| `a` | toggle arm | `k` | disarm | +| `s` | toggle sit/stand | `g` | tripod ↔ wave | +| `m` | direct ↔ heading-hold | `t` | right stick rotate ↔ translate | +| `r` | remote gyro on/off | `b` | robot gyro on/off | +| `+` / `-` | speed ±25 | `z` | zero handset level | +| `c` | start / save calibration | `x` | cancel calibration | +| `p` | print status (raw + mapped sticks, IMU, link, state, cal) | `d` | toggle 10 Hz packet dump | +| `R` | reset stick cal to Config defaults | | | + +## Bench-check checklist + +1. **Stick directions:** with `p`, confirm each logical axis reads + positive for right / forward / counterclockwise twist. Flip it in + `STICK_INVERT[]` if not. +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` / + `IMU_AXIS_SIGN`. +3. **Nextion baud:** `bauds=115200` must be in Program.s, or the display + shows nothing useful. +4. **Radio:** with the robot powered, `tLink` / `p` should show ~100%. If + it reads NO LINK, check that both boards were built from the same + `HexapodProtocol.h`. The static payload size is part of the handshake. +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. diff --git a/2026-current/remote/RadioLink.cpp b/2026-current/remote/RadioLink.cpp new file mode 100644 index 0000000..1a870d6 --- /dev/null +++ b/2026-current/remote/RadioLink.cpp @@ -0,0 +1,42 @@ +#include "RadioLink.h" +#include "Config.h" +#include +#include + +static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_PA_LOW, + "HexapodProtocol.h radio enum values out of sync with RF24.h"); + +static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN); + +bool RadioLink::begin() { + present_ = radio.begin(); + if (present_) { + radio.setPayloadSize(HXP_RADIO_PAYLOAD); // must match the robot exactly (static payloads) + radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL); + radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE); + radio.setChannel(HXP_RADIO_CHANNEL); + radio.setRetries(RADIO_RETRY_DELAY, RADIO_RETRY_COUNT); + radio.openWritingPipe(HXP_RADIO_ADDRESS); + radio.stopListening(); + } + return present_; +} + +bool RadioLink::send(const CmdPacket& p) { + if (!present_) return false; + bool acked = radio.write(&p, sizeof(p)); + history_ = (history_ << 1) | (acked ? 1u : 0u); + if (acked) { + last_ack_ms_ = millis(); + ever_acked_ = true; + } + return acked; +} + +void RadioLink::update(uint32_t now_ms) { + link_ok_ = present_ && ever_acked_ && (now_ms - last_ack_ms_) < LINK_LOST_MS; +} + +uint8_t RadioLink::qualityPct() const { + return (uint8_t)(__builtin_popcount(history_) * 100 / 32); +} diff --git a/2026-current/remote/RadioLink.h b/2026-current/remote/RadioLink.h new file mode 100644 index 0000000..81ecb92 --- /dev/null +++ b/2026-current/remote/RadioLink.h @@ -0,0 +1,22 @@ +#pragma once +#include + +// 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. +class RadioLink { +public: + bool begin(); // false if the radio chip didn't respond + bool send(const CmdPacket& p); // blocking, returns true if acked + void update(uint32_t now_ms); // refreshes linkOk() + + bool present() const { return present_; } + bool linkOk() const { return link_ok_; } + uint8_t qualityPct() const; // acked share of the last 32 packets + +private: + bool present_ = false; + bool link_ok_ = false; + uint32_t last_ack_ms_ = 0; + bool ever_acked_ = false; + uint32_t history_ = 0; // 1 bit per packet, newest in bit 0 +}; diff --git a/2026-current/remote/RemoteIMU.cpp b/2026-current/remote/RemoteIMU.cpp new file mode 100644 index 0000000..a6f0416 --- /dev/null +++ b/2026-current/remote/RemoteIMU.cpp @@ -0,0 +1,81 @@ +#include "RemoteIMU.h" +#include "Config.h" +#include + +// MPU6050 register map, default +/-2g / +/-250deg/s ranges (same part and +// scale factors as robot/RobotIMU.cpp). +static constexpr uint8_t REG_CONFIG = 0x1A; +static constexpr uint8_t REG_PWR_MGMT_1 = 0x6B; +static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B; + +static bool writeReg(uint8_t reg, uint8_t val) { + Wire.beginTransmission(IMU_ADDR); + Wire.write(reg); + Wire.write(val); + return Wire.endTransmission() == 0; +} + +bool RemoteIMU::begin() { + Wire.begin(); + Wire.setClock(400000); + ok_ = writeReg(REG_PWR_MGMT_1, 0x00); // wake out of sleep + if (!ok_) return false; + writeReg(REG_CONFIG, 0x03); // 44 Hz DLPF: a handheld remote doesn't need more bandwidth + delay(50); + + // Gyro bias: average while the remote sits still at power-on. + float sum[3] = {}; + int got = 0; + for (int n = 0; n < IMU_GYRO_BIAS_SAMPLES; n++) { + float a[3], g[3]; + if (readRaw(a, g)) { + for (int k = 0; k < 3; k++) sum[k] += g[k]; + got++; + } + delay(2); + } + if (got > 0) + for (int k = 0; k < 3; k++) gyro_bias_[k] = sum[k] / got; + + // Seed the filter from the accelerometer so it doesn't slew in from 0, + // and treat the power-on attitude as level until the operator re-zeroes. + float a[3], g[3]; + if (readRaw(a, g)) { + roll_deg_ = degrees(atan2f(a[1], a[2])); + pitch_deg_ = degrees(atan2f(-a[0], sqrtf(a[1] * a[1] + a[2] * a[2]))); + } + zeroLevel(); + return true; +} + +bool RemoteIMU::readRaw(float a[3], float g[3]) { + Wire.beginTransmission(IMU_ADDR); + Wire.write(REG_ACCEL_XOUT_H); + if (Wire.endTransmission(false) != 0) return false; + Wire.requestFrom((int)IMU_ADDR, 14); + if (Wire.available() < 14) return false; + + int16_t raw[7]; + for (int k = 0; k < 7; k++) raw[k] = (Wire.read() << 8) | Wire.read(); // ax ay az temp gx gy gz + + float chip_a[3] = { raw[0] / 16384.0f, raw[1] / 16384.0f, raw[2] / 16384.0f }; + float chip_g[3] = { raw[4] / 131.0f, raw[5] / 131.0f, raw[6] / 131.0f }; + + for (int k = 0; k < 3; k++) { + a[k] = chip_a[IMU_AXIS_FROM_CHIP[k]] * IMU_AXIS_SIGN[k]; + g[k] = chip_g[IMU_AXIS_FROM_CHIP[k]] * IMU_AXIS_SIGN[k] - gyro_bias_[k]; + } + return true; +} + +void RemoteIMU::update(float dt) { + if (!ok_) return; + float a[3], g[3]; + if (!readRaw(a, g)) return; + + float accel_roll = degrees(atan2f(a[1], a[2])); + float accel_pitch = degrees(atan2f(-a[0], sqrtf(a[1] * a[1] + a[2] * a[2]))); + + roll_deg_ = IMU_ALPHA * (roll_deg_ + g[0] * dt) + (1 - IMU_ALPHA) * accel_roll; + pitch_deg_ = IMU_ALPHA * (pitch_deg_ + g[1] * dt) + (1 - IMU_ALPHA) * accel_pitch; +} diff --git a/2026-current/remote/RemoteIMU.h b/2026-current/remote/RemoteIMU.h new file mode 100644 index 0000000..2ce5216 --- /dev/null +++ b/2026-current/remote/RemoteIMU.h @@ -0,0 +1,28 @@ +#pragma once +#include + +// Handset attitude from an MPU6050: same complementary filter as the robot's +// RobotIMU, plus a mounting-orientation remap, boot-time gyro bias capture, +// and a "level" reference so the operator's natural holding angle reads as 0. +class RemoteIMU { +public: + bool begin(); // false if the IMU didn't respond; hold the remote still during this + void update(float dt); // no-op if the IMU never came up + bool ok() const { return ok_; } + + // Tilt relative to the zeroed level pose, degrees, handset frame + // (+roll = right side down, +pitch = far edge down). + float rollDeg() const { return roll_deg_ - zero_roll_; } + float pitchDeg() const { return pitch_deg_ - zero_pitch_; } + + void zeroLevel() { zero_roll_ = roll_deg_; zero_pitch_ = pitch_deg_; } + +private: + float roll_deg_ = 0, pitch_deg_ = 0; + float zero_roll_ = 0, zero_pitch_ = 0; + float gyro_bias_[3] = {}; + bool ok_ = false; + + // Reads and remaps into the handset frame. Accel in g, gyro in deg/s. + bool readRaw(float a[3], float g[3]); +}; diff --git a/2026-current/remote/RemoteState.cpp b/2026-current/remote/RemoteState.cpp new file mode 100644 index 0000000..7335c3d --- /dev/null +++ b/2026-current/remote/RemoteState.cpp @@ -0,0 +1,114 @@ +#include "RemoteState.h" + +void RemoteState::begin() { + speed_.reset(speed_target_); + tilt_blend_.reset(0); +} + +void RemoteState::setMessage(const char* msg, uint32_t now_ms) { + message_ = msg; + message_until_ms_ = now_ms + STATUS_MESSAGE_MS; +} + +// Disarming always clears the stand request: the robot arms into SIT, and +// re-arming must never make it stand up on its own. +void RemoteState::disarm() { + armed_ = false; + stand_ = false; +} + +bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) { + const StickFrame& sticks = stick_hw.frame(); + switch (e.cmd) { + case UiCmd::None: + return false; + + case UiCmd::Disarm: + disarm(); + return true; + + case UiCmd::ToggleArm: + if (armed_) { disarm(); return true; } + if (calibrating_) { setMessage("Finish calibration first", now); return false; } + if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; } + armed_ = true; + return true; + + case UiCmd::ToggleStand: + if (!armed_) { setMessage("Arm first", now); return false; } + stand_ = !stand_; + return true; + + case UiCmd::ToggleGait: gait_wave_ = !gait_wave_; return true; + case UiCmd::ToggleWalkMode: mode_b_ = !mode_b_; return true; + case UiCmd::ToggleRStickXlate: rstick_xlate_ = !rstick_xlate_; return true; + case UiCmd::ToggleRobotGyro: robot_gyro_ = !robot_gyro_; return true; + + case UiCmd::ToggleRemoteGyro: + if (!remote_gyro_ && !imu.ok()) { setMessage("Remote IMU not detected", now); return false; } + remote_gyro_ = !remote_gyro_; + // Re-zero on enable: whatever angle you're holding the remote at when + // you switch it on becomes "level", so the body doesn't lurch. + if (remote_gyro_) imu.zeroLevel(); + return true; + + case UiCmd::SetSpeed: speed_target_ = e.value; return true; + case UiCmd::SpeedUp: speed_target_ = (uint8_t)min(255, speed_target_ + SPEED_SCALE_STEP); return true; + case UiCmd::SpeedDown: speed_target_ = (uint8_t)max(0, speed_target_ - SPEED_SCALE_STEP); return true; + + case UiCmd::ZeroGyro: + imu.zeroLevel(); + setMessage("Remote level zeroed", now); + return true; + + case UiCmd::CalStartSave: + if (!calibrating_) { + if (armed_) { setMessage("Disarm to calibrate", now); return false; } + stick_hw.startCalibration(); + calibrating_ = true; + setMessage("Sweep all axes to limits, release, SAVE", now); + return true; + } + if (!stick_hw.saveCalibration()) { setMessage("Sweep too small - keep sweeping", now); return false; } + calibrating_ = false; + setMessage("Calibration saved", now); + return true; + + case UiCmd::CalCancel: + if (!calibrating_) return false; + stick_hw.cancelCalibration(); + calibrating_ = false; + setMessage("Calibration cancelled", now); + return true; + } + return false; +} + +void RemoteState::update(float dt, bool link_ok, uint32_t now_ms) { + // 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 + // once the link is back. + if (!link_ok && link_was_ok_ && armed_) { + disarm(); + setMessage("Link lost - disarmed", now_ms); + } + link_was_ok_ = link_ok; + + speed_.setTarget(speed_target_); + speed_.update(dt); + tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f); + tilt_blend_.update(dt); +} + +void RemoteState::fillView(UiView& v, uint32_t now_ms) const { + v.armed = armed_; + v.stand = stand_; + v.gait_wave = gait_wave_; + v.mode_b = mode_b_; + v.rstick_xlate = rstick_xlate_; + v.remote_gyro = remote_gyro_; + v.robot_gyro = robot_gyro_; + v.calibrating = calibrating_; + v.speed_target = speed_target_; + v.message = message(now_ms); +} diff --git a/2026-current/remote/RemoteState.h b/2026-current/remote/RemoteState.h new file mode 100644 index 0000000..ed221a2 --- /dev/null +++ b/2026-current/remote/RemoteState.h @@ -0,0 +1,59 @@ +#pragma once +#include "UiTypes.h" +#include "Sticks.h" +#include "RemoteIMU.h" +#include "SmoothedValue.h" +#include "Config.h" + +// Operator-facing state of the remote: every toggle the touchscreen can +// flip, the interlocks around them, and the smoothed setpoints. It does not +// know the wire format - PacketBuilder turns this into a CmdPacket. +class RemoteState { +public: + void begin(); + + // Applies one operator request. Sticks are needed for the arm interlock + // and calibration, the IMU for re-zeroing. Returns true if accepted; + // rejections leave an explanation in message(). + bool handle(const UiEvent& e, Sticks& sticks, RemoteIMU& imu, uint32_t now_ms); + + // Advances smoothers and link-loss handling. Call once per TX tick. + void update(float dt, bool link_ok, uint32_t now_ms); + + bool armed() const { return armed_; } + bool standRequested() const { return stand_; } + bool gaitWave() const { return gait_wave_; } + bool modeB() const { return mode_b_; } + bool rstickXlate() const { return rstick_xlate_; } + bool remoteGyro() const { return remote_gyro_; } + bool robotGyro() const { return robot_gyro_; } + + uint8_t speedTarget() const { return speed_target_; } + uint8_t speedCurrent() const { return (uint8_t)lroundf(speed_.current); } + float remoteTiltBlend() const { return tilt_blend_.current; } // 0..1, fades on the gyro toggle + + 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 fillView(UiView& v, uint32_t now_ms) const; + +private: + bool armed_ = false; + bool stand_ = false; + bool gait_wave_ = false; + bool mode_b_ = false; + bool rstick_xlate_ = false; + bool remote_gyro_ = false; + bool robot_gyro_ = false; + bool calibrating_ = false; + + uint8_t speed_target_ = SPEED_SCALE_DEFAULT; + SmoothedValue speed_{SPEED_SCALE_RATE_PER_SEC}; + SmoothedValue tilt_blend_{REMOTE_TILT_FADE_PER_SEC}; + + bool link_was_ok_ = false; + const char* message_ = ""; + uint32_t message_until_ms_ = 0; + + void disarm(); +}; diff --git a/2026-current/remote/SerialConsole.cpp b/2026-current/remote/SerialConsole.cpp new file mode 100644 index 0000000..24f8242 --- /dev/null +++ b/2026-current/remote/SerialConsole.cpp @@ -0,0 +1,30 @@ +#include "SerialConsole.h" +#include + +bool SerialConsole::poll(UiEvent& ui, Local& local) { + ui = UiEvent{}; + local = Local::None; + while (Serial.available()) { + char c = Serial.read(); + switch (c) { + case 'a': ui.cmd = UiCmd::ToggleArm; return true; + case 'k': ui.cmd = UiCmd::Disarm; return true; + case 's': ui.cmd = UiCmd::ToggleStand; return true; + case 'g': ui.cmd = UiCmd::ToggleGait; return true; + case 'm': ui.cmd = UiCmd::ToggleWalkMode; return true; + case 't': ui.cmd = UiCmd::ToggleRStickXlate; return true; + case 'r': ui.cmd = UiCmd::ToggleRemoteGyro; return true; + case 'b': ui.cmd = UiCmd::ToggleRobotGyro; return true; + case '+': case '=': ui.cmd = UiCmd::SpeedUp; return true; + case '-': case '_': ui.cmd = UiCmd::SpeedDown; return true; + case 'z': ui.cmd = UiCmd::ZeroGyro; return true; + case 'c': ui.cmd = UiCmd::CalStartSave; return true; + case 'x': ui.cmd = UiCmd::CalCancel; return true; + case 'p': local = Local::PrintStatus; return true; + case 'd': local = Local::ToggleDump; return true; + case 'R': local = Local::ResetCal; return true; + default: break; // ignore line endings and unknown keys + } + } + return false; +} diff --git a/2026-current/remote/SerialConsole.h b/2026-current/remote/SerialConsole.h new file mode 100644 index 0000000..01fd9e0 --- /dev/null +++ b/2026-current/remote/SerialConsole.h @@ -0,0 +1,14 @@ +#pragma once +#include "UiTypes.h" + +// USB serial console (115200). Mirrors every touchscreen control with a +// single keystroke, so the remote can be bench-tested with no Nextion +// attached, plus a few diagnostics. Key map in remote/README.md. +class SerialConsole { +public: + enum class Local : uint8_t { None, PrintStatus, ToggleDump, ResetCal }; + + // Returns true with either `ui` set (a normal operator command) or + // `local` set (a console-only diagnostic the sketch handles itself). + bool poll(UiEvent& ui, Local& local); +}; diff --git a/2026-current/remote/SmoothedValue.h b/2026-current/remote/SmoothedValue.h new file mode 100644 index 0000000..d92ae52 --- /dev/null +++ b/2026-current/remote/SmoothedValue.h @@ -0,0 +1,26 @@ +#pragma once + +// Rate-limited setpoint smoother - identical to robot/SmoothedValue.h (kept +// as a copy rather than moved to shared/ so the robot sketch stays untouched). +// Used for UI-driven setpoints that would otherwise step: speed scale and +// the remote-tilt fade. +struct SmoothedValue { + float current = 0; + float target = 0; + float max_rate; // units per second + + explicit SmoothedValue(float rate = 100.0f) : max_rate(rate) {} + + void setTarget(float t) { target = t; } + + float update(float dt) { + float delta = target - current; + float max_step = max_rate * dt; + if (delta > max_step) delta = max_step; + else if (delta < -max_step) delta = -max_step; + current += delta; + return current; + } + + void reset(float v) { current = target = v; } +}; diff --git a/2026-current/remote/Sticks.cpp b/2026-current/remote/Sticks.cpp new file mode 100644 index 0000000..01d81b2 --- /dev/null +++ b/2026-current/remote/Sticks.cpp @@ -0,0 +1,155 @@ +#include "Sticks.h" +#include + +// EEPROM layout: one record at address 0. Bump CAL_VERSION if AxisCal or +// the axis order ever changes so stale data is ignored instead of misread. +static constexpr uint32_t CAL_MAGIC = 0x48585043; // "HXPC" +static constexpr uint16_t CAL_VERSION = 1; +static constexpr int CAL_EEPROM_ADDR = 0; + +struct StoredCal { + uint32_t magic; + uint16_t version; + AxisCal axes[NUM_AXES]; + uint16_t checksum; +}; + +static uint16_t checksumOf(const StoredCal& s) { + const uint8_t* p = reinterpret_cast(&s); + uint16_t sum = 0; + for (size_t i = 0; i < offsetof(StoredCal, checksum); i++) sum = (sum << 1 | sum >> 15) ^ p[i]; + return sum; +} + +bool StickFrame::allCentered(int16_t tolerance) const { + for (int i = 0; i < NUM_AXES; i++) + if (abs(v[i]) > tolerance) return false; + return true; +} + +bool Sticks::begin() { + analogReadResolution(ADC_BITS); + analogReadAveraging(ADC_AVERAGING); + + if (!loadFromEeprom()) applyDefaults(); + + bool all_ok = true; + for (int i = 0; i < NUM_AXES; i++) { + int16_t rest = readAveraged(i, 32); + filtered_[i] = rest; + if (abs(rest - cal_[i].center) <= CENTER_CAPTURE_TOLERANCE) { + cal_[i].center = rest; + } else { + all_ok = false; // someone's holding a stick, or the stored cal is off - keep stored center + } + } + return all_ok; +} + +int16_t Sticks::readAveraged(int axis, int samples) { + int32_t sum = 0; + for (int n = 0; n < samples; n++) sum += analogRead(STICK_PINS[axis]); + return (int16_t)(sum / samples); +} + +// Raw counts -> -1..1 (piecewise around center, so asymmetric pots still +// reach full scale both ways) -> invert -> deadzone with rescale -> expo. +int16_t Sticks::mapAxis(int axis, float raw) const { + const AxisCal& c = cal_[axis]; + float n; + if (raw >= c.center) n = (raw - c.center) / (float)max(1, c.max - c.center); + else n = (raw - c.center) / (float)max(1, c.center - c.min); + n = constrain(n, -1.0f, 1.0f); + if (STICK_INVERT[axis]) n = -n; + + float dz = STICK_DEADZONE[axis]; + float mag = fabsf(n); + if (mag <= dz) return 0; + mag = (mag - dz) / (1.0f - dz); + + float e = STICK_EXPO[axis]; + mag = (1.0f - e) * mag + e * mag * mag * mag; + + return (int16_t)lroundf(copysignf(mag, n) * 1000.0f); +} + +void Sticks::update() { + for (int i = 0; i < NUM_AXES; i++) { + float r = analogRead(STICK_PINS[i]); + filtered_[i] += STICK_FILTER_ALPHA * (r - filtered_[i]); + + if (calibrating_) { + int16_t v = (int16_t)filtered_[i]; + sweep_[i].min = min(sweep_[i].min, v); + sweep_[i].max = max(sweep_[i].max, v); + frame_.v[i] = 0; // never command motion while the operator is sweeping sticks + } else { + frame_.v[i] = mapAxis(i, filtered_[i]); + } + } +} + +void Sticks::startCalibration() { + for (int i = 0; i < NUM_AXES; i++) { + int16_t v = (int16_t)filtered_[i]; + sweep_[i] = { v, v, v }; + } + calibrating_ = true; +} + +bool Sticks::saveCalibration() { + if (!calibrating_) return false; + AxisCal next[NUM_AXES]; + for (int i = 0; i < NUM_AXES; i++) { + // Sticks are released when SAVE is pressed, so the current reading is center. + int16_t center = (int16_t)filtered_[i]; + if (center - sweep_[i].min < CAL_MIN_HALF_SPAN || sweep_[i].max - center < CAL_MIN_HALF_SPAN) + return false; // stay in calibration mode so the operator can keep sweeping + // Pull the ends in slightly so full deflection reliably reaches 1000. + int16_t inset_lo = (center - sweep_[i].min) / 50; + int16_t inset_hi = (sweep_[i].max - center) / 50; + next[i] = { (int16_t)(sweep_[i].min + inset_lo), center, (int16_t)(sweep_[i].max - inset_hi) }; + } + memcpy(cal_, next, sizeof(cal_)); + saveToEeprom(); + calibrating_ = false; + return true; +} + +void Sticks::cancelCalibration() { calibrating_ = false; } + +void Sticks::applyDefaults() { + for (int i = 0; i < NUM_AXES; i++) + cal_[i] = { STICK_DEFAULT_MIN, STICK_DEFAULT_CENTER, STICK_DEFAULT_MAX }; +} + +void Sticks::resetCalibrationToDefaults() { + applyDefaults(); + StoredCal blank{}; + EEPROM.put(CAL_EEPROM_ADDR, blank); // magic = 0 -> ignored on next boot +} + +bool Sticks::loadFromEeprom() { + StoredCal s; + EEPROM.get(CAL_EEPROM_ADDR, s); + if (s.magic != CAL_MAGIC || s.version != CAL_VERSION || s.checksum != checksumOf(s)) return false; + memcpy(cal_, s.axes, sizeof(cal_)); + return true; +} + +void Sticks::saveToEeprom() { + StoredCal s{}; + s.magic = CAL_MAGIC; + s.version = CAL_VERSION; + memcpy(s.axes, cal_, sizeof(cal_)); + s.checksum = checksumOf(s); + EEPROM.put(CAL_EEPROM_ADDR, s); +} + +void Sticks::printCalibration(Stream& s) const { + static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TWIST", "R_X", "R_Y", "R_TWIST" }; + s.println("// stick calibration {min, center, max}:"); + for (int i = 0; i < NUM_AXES; i++) { + s.printf("// %-8s { %4d, %4d, %4d }\n", names[i], cal_[i].min, cal_[i].center, cal_[i].max); + } +} diff --git a/2026-current/remote/Sticks.h b/2026-current/remote/Sticks.h new file mode 100644 index 0000000..c5e4091 --- /dev/null +++ b/2026-current/remote/Sticks.h @@ -0,0 +1,47 @@ +#pragma once +#include "Config.h" + +struct AxisCal { + int16_t min, center, max; // raw ADC counts +}; + +// Processed stick values, -1000..1000 per logical axis (see StickAxis): +// X + = right, Y + = forward, TWIST + = counterclockwise. +struct StickFrame { + int16_t v[NUM_AXES] = {}; + bool allCentered(int16_t tolerance) const; +}; + +class Sticks { +public: + // Loads calibration from EEPROM (or Config defaults), then captures the + // resting center of each axis. Returns false if a stick looked deflected + // at power-on (center capture skipped for that axis). + bool begin(); + void update(); // read + filter + map all axes + const StickFrame& frame() const { return frame_; } + int16_t raw(int axis) const { return (int16_t)filtered_[axis]; } + const AxisCal& cal(int axis) const { return cal_[axis]; } + + // Sweep calibration: start, move every stick to all its limits, release + // all sticks, then save. Output is held at zero while calibrating. + void startCalibration(); + bool saveCalibration(); // false = sweep too small, nothing saved + void cancelCalibration(); + bool calibrating() const { return calibrating_; } + void resetCalibrationToDefaults(); // also clears EEPROM + void printCalibration(Stream& s) const; // paste-able into Config.h + +private: + AxisCal cal_[NUM_AXES]; + AxisCal sweep_[NUM_AXES]; + float filtered_[NUM_AXES] = {}; + StickFrame frame_; + bool calibrating_ = false; + + int16_t readAveraged(int axis, int samples); + int16_t mapAxis(int axis, float raw) const; + void applyDefaults(); + bool loadFromEeprom(); + void saveToEeprom(); +}; diff --git a/2026-current/remote/UiTypes.h b/2026-current/remote/UiTypes.h new file mode 100644 index 0000000..f6870ae --- /dev/null +++ b/2026-current/remote/UiTypes.h @@ -0,0 +1,45 @@ +#pragma once +#include + +// Operator commands. Produced by the Nextion touchscreen (NextionUI) and by +// the USB serial console (SerialConsole) through the same path, so the +// remote can be fully exercised on the bench without the display attached. +enum class UiCmd : uint8_t { + None = 0, + ToggleArm, + Disarm, // unconditional - the big red button + ToggleStand, + ToggleGait, // tripod <-> wave + ToggleWalkMode, // direct (A) <-> heading-hold (B) + ToggleRStickXlate, // right stick: rotation <-> translation + ToggleRemoteGyro, + ToggleRobotGyro, + SetSpeed, // value = 0..255 + SpeedUp, // serial console only + SpeedDown, // serial console only + ZeroGyro, // capture the handset's current attitude as "level" + CalStartSave, // start sweep calibration, or save it if running + CalCancel, +}; + +struct UiEvent { + UiCmd cmd = UiCmd::None; + uint8_t value = 0; +}; + +// Everything the display shows. The remote is authoritative: the Nextion +// only sends requests, and always renders what's in here. +struct UiView { + bool armed = false; + bool stand = false; + bool gait_wave = false; + bool mode_b = false; + bool rstick_xlate = false; + bool remote_gyro = false; + bool robot_gyro = false; + bool calibrating = false; + uint8_t speed_target = 0; + bool link_ok = false; + uint8_t link_pct = 0; + const char* message = ""; // always a string literal - NextionUI diffs it by pointer +}; diff --git a/2026-current/remote/remote.ino b/2026-current/remote/remote.ino new file mode 100644 index 0000000..e102e75 --- /dev/null +++ b/2026-current/remote/remote.ino @@ -0,0 +1,117 @@ +#include "Config.h" +#include "Sticks.h" +#include "RemoteIMU.h" +#include "RemoteState.h" +#include "PacketBuilder.h" +#include "RadioLink.h" +#include "NextionUI.h" +#include "SerialConsole.h" + +// Hexapod remote (Teensy 4.1): reads two 3-axis sticks and the handset +// MPU6050, takes operator commands from the Nextion touchscreen (and the USB +// serial console), and transmits a CmdPacket to the robot at TX_HZ. +// See README.md for wiring, calibration and the serial key map. + +Sticks sticks; +RemoteIMU imu; +RemoteState state; +PacketBuilder builder; +RadioLink radio_link; +NextionUI nextion; +SerialConsole console; + +uint32_t last_tx_us = 0; +bool dump_packets = false; +uint32_t last_dump_ms = 0; +CmdPacket last_packet; + +static void printStatus() { + static const char* const names[NUM_AXES] = { "L_X", "L_Y", "L_TW", "R_X", "R_Y", "R_TW" }; + Serial.println("---- remote status ----"); + for (int i = 0; i < NUM_AXES; i++) + Serial.printf("%-5s raw %4d -> %5d\n", names[i], sticks.raw(i), sticks.frame().v[i]); + Serial.printf("IMU %s roll %6.1f pitch %6.1f (deg, vs zeroed level)\n", + imu.ok() ? "ok" : "MISSING", imu.rollDeg(), imu.pitchDeg()); + Serial.printf("radio %s link %s %u%%\n", radio_link.present() ? "ok" : "MISSING", + radio_link.linkOk() ? "OK" : "DOWN", radio_link.qualityPct()); + Serial.printf("armed %d stand %d gait %s mode %s rstick %s remote_gyro %d robot_gyro %d speed %u->%u\n", + state.armed(), state.standRequested(), state.gaitWave() ? "wave" : "tripod", + state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot", + state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent()); + sticks.printCalibration(Serial); +} + +static void dumpPacket(const CmdPacket& p) { + Serial.printf("#%3u vx %5d vy %5d yaw %5d | r %5d %5d %5d | tilt %5d %5d | spd %3u flags %02X\n", + p.seq, p.vx, p.vy, p.yaw_rate, p.rx, p.ry, p.rz, + p.remote_roll, p.remote_pitch, p.speed_scale, p.flags); +} + +static void pollInputs(uint32_t now_ms) { + UiEvent ev; + while (nextion.poll(ev)) state.handle(ev, sticks, imu, now_ms); + + SerialConsole::Local local; + while (console.poll(ev, local)) { + if (ev.cmd != UiCmd::None) { + bool ok = state.handle(ev, sticks, imu, now_ms); + if (!ok && state.message(now_ms)[0]) Serial.println(state.message(now_ms)); + } + switch (local) { + case SerialConsole::Local::PrintStatus: printStatus(); break; + case SerialConsole::Local::ToggleDump: dump_packets = !dump_packets; break; + case SerialConsole::Local::ResetCal: + if (state.armed()) { Serial.println("disarm first"); break; } + sticks.resetCalibrationToDefaults(); + Serial.println("stick calibration reset to Config.h defaults"); + break; + case SerialConsole::Local::None: break; + } + } +} + +void setup() { + Serial.begin(115200); + nextion.begin(); + + // Keep the remote still and both sticks released during power-on: the + // IMU captures its gyro bias and the sticks capture their centers here. + if (!sticks.begin()) Serial.println("stick center capture skipped on some axes - was a stick held at power-on?"); + if (!imu.begin()) Serial.println("remote IMU not responding - remote-gyro tilt unavailable"); + if (!radio_link.begin()) Serial.println("radio not responding - check nRF24 wiring/power"); + + state.begin(); + last_tx_us = micros(); +} + +void loop() { + uint32_t now_ms = millis(); + + // UI input is drained every pass, not just on TX ticks, so no touch + // events are lost between packets. + pollInputs(now_ms); + + uint32_t now_us = micros(); + float dt = (now_us - last_tx_us) / 1e6f; + if (dt < (1.0f / TX_HZ)) return; // hold to the fixed TX rate + last_tx_us = now_us; + + sticks.update(); + imu.update(dt); + radio_link.update(now_ms); + state.update(dt, radio_link.linkOk(), now_ms); + + last_packet = builder.build(sticks.frame(), imu, state, now_ms); + radio_link.send(last_packet); + + UiView view; + state.fillView(view, now_ms); + view.link_ok = radio_link.linkOk(); + view.link_pct = radio_link.qualityPct(); + nextion.render(view, now_ms); + + if (dump_packets && now_ms - last_dump_ms >= 100) { + last_dump_ms = now_ms; + dumpPacket(last_packet); + } +} diff --git a/2026-current/robot/Comms.cpp b/2026-current/robot/Comms.cpp index 21089f7..eebe060 100644 --- a/2026-current/robot/Comms.cpp +++ b/2026-current/robot/Comms.cpp @@ -3,15 +3,19 @@ #include #include +static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_PA_LOW, + "HexapodProtocol.h radio enum values out of sync with RF24.h"); + static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN); bool Comms::begin() { radio_present_ = radio.begin(); if (radio_present_) { - radio.openReadingPipe(1, RADIO_ADDRESS); - radio.setPALevel(RF24_PA_LOW); - radio.setDataRate(RF24_250KBPS); - radio.setChannel(96); // pick one clear of your local Wi-Fi + radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all + radio.openReadingPipe(1, HXP_RADIO_ADDRESS); + radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL); + radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE); + radio.setChannel(HXP_RADIO_CHANNEL); radio.startListening(); } return radio_present_; diff --git a/2026-current/robot/Comms.h b/2026-current/robot/Comms.h index b29f220..2545ec6 100644 --- a/2026-current/robot/Comms.h +++ b/2026-current/robot/Comms.h @@ -1,26 +1,6 @@ #pragma once #include - -// Remote -> robot, sent at a fixed rate. Kept as the single source of truth -// here so a future remote sketch just has to fill this same struct and -// radio.write() it - nothing on the robot side needs to change. -struct CmdPacket { - int16_t vx = 0, vy = 0; // left stick: strafe / forward-back, -1000..1000 - int16_t yaw_rate = 0; // left stick twist, -1000..1000 - int16_t rx = 0, ry = 0, rz = 0; // right stick: roll/pitch/yaw OR X/Y/height, per flags - uint8_t speed_scale = 128; // 0-255 - uint8_t flags = 0; // bit-packed, see FLAG_* below - uint8_t seq = 0; -}; - -constexpr uint8_t FLAG_ARMED = 1 << 0; -constexpr uint8_t FLAG_STAND = 1 << 1; // 0 = sit requested, 1 = stand requested -constexpr uint8_t FLAG_WALK = 1 << 2; // stick past deadzone -> walking -constexpr uint8_t FLAG_MODE_B = 1 << 3; // 0 = mode A direct, 1 = mode B heading-hold -constexpr uint8_t FLAG_RSTICK_XLATE = 1 << 4; // 0 = right stick = rotation, 1 = translation -constexpr uint8_t FLAG_GAIT_WAVE = 1 << 5; // 0 = tripod requested, 1 = wave requested -constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled -constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled +#include // CmdPacket + FLAG_* + radio settings, shared with the remote (shared/) class Comms { public: diff --git a/2026-current/robot/Config.h b/2026-current/robot/Config.h index 2d67109..4e93717 100644 --- a/2026-current/robot/Config.h +++ b/2026-current/robot/Config.h @@ -49,9 +49,10 @@ constexpr float SWING_LIFT_MM = 35.0f; constexpr float CONTROL_HZ = 60.0f; // ---- Radio (nRF24L01) ---- +// Address/channel/data rate/PA/payload size live in shared/HexapodProtocol +// so the robot and remote can't drift apart. Only this board's wiring is here. constexpr uint8_t RADIO_CE_PIN = 33; constexpr uint8_t RADIO_CSN_PIN = 34; -constexpr uint8_t RADIO_ADDRESS[6] = "HXPD1"; // must match the remote once it exists constexpr float LINK_TIMEOUT_SEC = 0.3f; // ---- Setpoint smoothing rates ---- diff --git a/2026-current/robot/InputMapper.cpp b/2026-current/robot/InputMapper.cpp index 8252a04..2f37267 100644 --- a/2026-current/robot/InputMapper.cpp +++ b/2026-current/robot/InputMapper.cpp @@ -56,5 +56,15 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) { out.pose.yaw_deg = rz * MAX_TILT_DEG; } + // Remote handset tilt feedforward: its own packet fields, gated only by + // FLAG_REMOTE_GYRO, added on top of whatever the right stick commands (so + // it works in both right-stick sub-modes). Total tilt stays within MAX_TILT_DEG. + if (out.remote_gyro_enabled) { + out.pose.roll_deg = constrain(out.pose.roll_deg + normalize(p.remote_roll) * MAX_TILT_DEG, + -MAX_TILT_DEG, MAX_TILT_DEG); + out.pose.pitch_deg = constrain(out.pose.pitch_deg + normalize(p.remote_pitch) * MAX_TILT_DEG, + -MAX_TILT_DEG, MAX_TILT_DEG); + } + return out; } diff --git a/2026-current/robot/README.md b/2026-current/robot/README.md index 41b7cdf..a3321de 100644 --- a/2026-current/robot/README.md +++ b/2026-current/robot/README.md @@ -1,16 +1,20 @@ # Hexapod robot firmware (Teensy 4.0) -This is the **robot-side** firmware only. It is fully functional on its own -(driven by the serial test harness below) and is already wired to receive -commands from a future remote over nRF24L01 — the packet format in -`Comms.h` is the contract the remote sketch must match. +This is the **robot-side** firmware. It is fully functional on its own +(driven by the serial test harness below) and receives commands from the +remote (`../remote/`) over nRF24L01. The packet format lives in +`../shared/HexapodProtocol/HexapodProtocol.h` — the single definition both +sketches compile against. -## Required libraries +## Building -Install via Arduino Library Manager (or Teensyduino's manager): +From the repo root (PlatformIO): `pio run -e robot`, flash with +`pio run -e robot -t upload`, monitor with `pio device monitor -e robot`. +RF24 is pulled in automatically via `lib_deps`; Servo ships with the Teensy +framework. -- **RF24** by TMRh20 — nRF24L01 radio driver -- **Servo** — bundled with Teensyduino, nothing to install +Arduino IDE also works if you copy `shared/HexapodProtocol/` into your +sketchbook `libraries/` folder and install **RF24** by TMRh20. ## Before you trust this on the floor @@ -26,8 +30,12 @@ your own robot and edit: - `SERVO_PINS` — must be PWM-capable pins on your Teensy 4.0 wiring - `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` — calibrate per servo, they vary batch to batch -- `RADIO_ADDRESS` / channel (set in `Comms.cpp`) — must match the remote - once it exists +- `RADIO_CE_PIN` / `RADIO_CSN_PIN` — your nRF24 wiring. Address, channel, + data rate, PA level and payload size are in `shared/HexapodProtocol` so + they can't drift from the remote. +- **Pin conflict:** `SERVO_PINS` leg 3 uses pins 11 and 12, which are the + Teensy 4.0's SPI MOSI/MISO that the nRF24 needs. Move those two servos + before wiring the radio. ## Bench-check checklist (do this before the first stand-up) @@ -50,6 +58,14 @@ for your build: If your chassis is meant to rest its belly on the ground instead, this needs a different, sequenced approach (fold legs up rather than just lowering). +5. **Gait travel direction** (`GaitEngine.cpp`) — the protocol defines + `+vx` = forward, `+vy` = left, `+yaw_rate` = turn left (CCW). As written, + the stance phase sweeps each foot from `neutral - half_stride` to + `neutral + half_stride`, i.e. the foot moves *with* the commanded + direction, which pushes the body the opposite way. Verify with `w` on the + bench; if the robot backs up, swap `start`/`end` in `GaitEngine::update` + (fixes translation and yaw together). The `z`/`c` harness keys were + written against the current behavior — recheck them after that fix. ## Testing without a remote yet @@ -61,8 +77,8 @@ keystrokes) and use: |-----|--------| | `a` | toggle armed | | `s` | toggle sit / stand | -| `w` / `x` | walk forward / backward | -| `q` / `e` | strafe left / right | +| `w` / `x` | walk forward / backward (`vx` ±) | +| `q` / `e` | strafe left / right (`vy` ±) | | `z` / `c` | yaw left / right | | `g` | toggle gait: tripod ↔ wave | | `m` | toggle walk mode: direct (A) ↔ heading-hold (B) | @@ -79,17 +95,20 @@ Arm (`a`), then stand (`s`), before trying to walk — the state machine enforces `DISARMED → SIT → STAND → WALKING` in order and will otherwise ignore walk commands. -## What's ready for the remote +## Link with the remote -- `Comms.h`'s `CmdPacket` struct and `FLAG_*` bit constants are the exact - wire format the remote must send (as an RF24 payload to `RADIO_ADDRESS` - on the same channel/data-rate/PA-level set in `Comms::begin()`). +- `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the + exact wire format. Both radios set a static payload size of + `sizeof(CmdPacket)`, so a robot and remote built from different protocol + versions won't receive each other at all (and the robot fails safe). - Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()` goes false and the main loop falls back to a zeroed `CmdPacket`, which disarms the robot. -- `FLAG_REMOTE_GYRO` / `FLAG_ROBOT_GYRO` independently gate whether the - remote's tilt feeds into body pose and whether the robot's own IMU - stabilizes it — matching the touchscreen toggles you described. +- The right stick's roll/pitch/yaw (or X/Y/Z) always apply. The remote's + handset tilt arrives in its own `remote_roll` / `remote_pitch` fields and + is added on top only while `FLAG_REMOTE_GYRO` is set, capped at + `MAX_TILT_DEG` total (`InputMapper`). `FLAG_ROBOT_GYRO` independently + enables the robot's own IMU stabilization. - `FLAG_RSTICK_XLATE` switches the right stick between rotation-axes and translation-axes interpretation, and `FLAG_MODE_B` switches between the two walk modes, all inside the existing `InputMapper`. @@ -108,6 +127,6 @@ robot/ ├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend ├── InputMapper.h/.cpp - CmdPacket -> twist/pose/mode decoding ├── Limits.h/.cpp - workspace clamp, joint rate limit, stance polygon check -├── Comms.h/.cpp - nRF24L01 receive + packet/flags definition +├── Comms.h/.cpp - nRF24L01 receive (packet definition is in shared/HexapodProtocol) └── ServoDriver.h/.cpp - servo attach/relax/write ``` diff --git a/2026-current/robot/robot.ino b/2026-current/robot/robot.ino index 01466b1..de7d43e 100644 --- a/2026-current/robot/robot.ino +++ b/2026-current/robot/robot.ino @@ -36,10 +36,11 @@ void pollSerialTestInput() { switch (c) { case 'a': test_packet.flags ^= FLAG_ARMED; break; case 's': test_packet.flags ^= FLAG_STAND; break; - case 'w': test_packet.vy = min(test_packet.vy + 200, 1000); test_packet.flags |= FLAG_WALK; break; - case 'x': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break; - case 'q': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break; - case 'e': test_packet.vx = min(test_packet.vx + 200, 1000); test_packet.flags |= FLAG_WALK; break; + // vx = forward (+X), vy = strafe left (+Y) - see HexapodProtocol.h axis convention. + case 'w': test_packet.vx = min(test_packet.vx + 200, 1000); test_packet.flags |= FLAG_WALK; break; + case 'x': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break; + case 'q': test_packet.vy = min(test_packet.vy + 200, 1000); test_packet.flags |= FLAG_WALK; break; + case 'e': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break; case 'z': test_packet.yaw_rate = max(test_packet.yaw_rate - 200, -1000); test_packet.flags |= FLAG_WALK; break; case 'c': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000); test_packet.flags |= FLAG_WALK; break; case 'g': test_packet.flags ^= FLAG_GAIT_WAVE; break; @@ -108,13 +109,9 @@ void loop() { gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD); - // Remote-gyro toggle off -> drop commanded tilt but keep XYZ offset/height. - PoseCommand pose_cmd = mapped.pose; - if (!mapped.remote_gyro_enabled) { - pose_cmd.roll_deg = 0; - pose_cmd.pitch_deg = 0; - } - pose.setCommand(pose_cmd); + // Stick pose + remote-tilt feedforward (gated on FLAG_REMOTE_GYRO) are + // already combined in InputMapper. + pose.setCommand(mapped.pose); pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0, mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0); pose.update(dt); diff --git a/2026-current/shared/HexapodProtocol/HexapodProtocol.h b/2026-current/shared/HexapodProtocol/HexapodProtocol.h new file mode 100644 index 0000000..5bd76b6 --- /dev/null +++ b/2026-current/shared/HexapodProtocol/HexapodProtocol.h @@ -0,0 +1,59 @@ +#pragma once +#include + +// ===================================================================== +// Remote -> robot wire protocol. SINGLE SOURCE OF TRUTH. +// +// Both robot/ and remote/ compile against this exact header (PlatformIO +// lib_dir = shared). Any change here changes the over-the-air format: +// rebuild and reflash BOTH boards together. The static payload size is set +// to sizeof(CmdPacket) on both radios, so a remote and robot running +// mismatched versions simply won't hear each other (the robot then fails +// safe on link timeout) instead of misinterpreting fields. +// +// Axis convention (right-handed body frame, viewed from above, like ROS +// REP-103): +X forward, +Y left, +Z up. Positive rotations follow the +// right-hand rule: +roll = right side down, +pitch = nose down, +// +yaw = counterclockwise (turn left). +// +// All analog fields are normalized -1000..1000. The robot owns the physical +// scaling (max mm/s, max tilt deg, max offset mm) in InputMapper, so the +// remote never needs to know robot geometry. +// ===================================================================== + +struct CmdPacket { + int16_t vx = 0; // left stick: + forward / - backward + int16_t vy = 0; // left stick: + strafe left / - strafe right + int16_t yaw_rate = 0; // left stick twist: + turn left (CCW) + int16_t rx = 0, ry = 0, rz = 0; // right stick: roll/pitch/yaw, or X/Y/Z offset if FLAG_RSTICK_XLATE + int16_t remote_roll = 0; // remote handset tilt (feedforward), only honored with FLAG_REMOTE_GYRO + int16_t remote_pitch = 0; + uint8_t speed_scale = 128; // 0-255, multiplies vx/vy + uint8_t flags = 0; // bit-packed, see FLAG_* below + uint8_t seq = 0; // increments every transmitted packet +}; + +// Both ends are Teensy (Cortex-M7, little-endian, 2-byte int16 alignment): +// 8 x int16 + 3 x uint8 = 19 bytes, padded to 20. If a different MCU family +// ever joins, pack the struct explicitly rather than trusting this. +static_assert(sizeof(CmdPacket) == 20, "CmdPacket layout changed - reflash both boards"); +static_assert(sizeof(CmdPacket) <= 32, "nRF24L01 payloads are limited to 32 bytes"); + +constexpr uint8_t FLAG_ARMED = 1 << 0; +constexpr uint8_t FLAG_STAND = 1 << 1; // 0 = sit requested, 1 = stand requested +constexpr uint8_t FLAG_WALK = 1 << 2; // left stick past deadzone -> walking +constexpr uint8_t FLAG_MODE_B = 1 << 3; // 0 = mode A direct, 1 = mode B heading-hold +constexpr uint8_t FLAG_RSTICK_XLATE = 1 << 4; // 0 = right stick = rotation, 1 = translation +constexpr uint8_t FLAG_GAIT_WAVE = 1 << 5; // 0 = tripod requested, 1 = wave requested +constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled +constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled + +// ---- 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_CHANNEL = 96; // 2400+96 MHz; pick one clear of your local Wi-Fi +constexpr uint8_t HXP_RADIO_PAYLOAD = sizeof(CmdPacket); +// 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; +// both Comms.cpp (robot) and RadioLink.cpp (remote) static_assert this. +constexpr uint8_t HXP_RADIO_DATA_RATE = 2; // RF24_250KBPS +constexpr uint8_t HXP_RADIO_PA_LEVEL = 1; // RF24_PA_LOW diff --git a/2026-current/shared/HexapodProtocol/library.json b/2026-current/shared/HexapodProtocol/library.json new file mode 100644 index 0000000..8d7871a --- /dev/null +++ b/2026-current/shared/HexapodProtocol/library.json @@ -0,0 +1,7 @@ +{ + "name": "HexapodProtocol", + "version": "2.0.0", + "description": "Remote->robot CmdPacket wire format and nRF24 link settings shared by robot/ and remote/.", + "frameworks": "arduino", + "platforms": "teensy" +}