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 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
ad77b06c36
commit
91dac16fca
30 files changed
+1706
-57
No files matched your search
@@ -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 <HexapodProtocol.h>`.
|
||||||
|
- 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/`.
|
||||||
@@ -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.
|
||||||
@@ -0,0 +1,110 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// =====================================================================
|
||||||
|
// 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
|
||||||
@@ -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`<br>`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.
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
#include "NextionUI.h"
|
||||||
|
#include <stdarg.h>
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
@@ -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:
|
||||||
|
// '#' <len> <cmd> [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);
|
||||||
|
};
|
||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,21 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <HexapodProtocol.h>
|
||||||
|
#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);
|
||||||
|
};
|
||||||
@@ -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.
|
||||||
@@ -0,0 +1,42 @@
|
|||||||
|
#include "RadioLink.h"
|
||||||
|
#include "Config.h"
|
||||||
|
#include <RF24.h>
|
||||||
|
#include <SPI.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
@@ -0,0 +1,22 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <HexapodProtocol.h>
|
||||||
|
|
||||||
|
// 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
|
||||||
|
};
|
||||||
@@ -0,0 +1,81 @@
|
|||||||
|
#include "RemoteIMU.h"
|
||||||
|
#include "Config.h"
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// 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]);
|
||||||
|
};
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
@@ -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();
|
||||||
|
};
|
||||||
@@ -0,0 +1,30 @@
|
|||||||
|
#include "SerialConsole.h"
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
@@ -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);
|
||||||
|
};
|
||||||
@@ -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; }
|
||||||
|
};
|
||||||
@@ -0,0 +1,155 @@
|
|||||||
|
#include "Sticks.h"
|
||||||
|
#include <EEPROM.h>
|
||||||
|
|
||||||
|
// 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<const uint8_t*>(&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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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();
|
||||||
|
};
|
||||||
@@ -0,0 +1,45 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// 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
|
||||||
|
};
|
||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -3,15 +3,19 @@
|
|||||||
#include <RF24.h>
|
#include <RF24.h>
|
||||||
#include <SPI.h>
|
#include <SPI.h>
|
||||||
|
|
||||||
|
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);
|
static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
|
||||||
|
|
||||||
bool Comms::begin() {
|
bool Comms::begin() {
|
||||||
radio_present_ = radio.begin();
|
radio_present_ = radio.begin();
|
||||||
if (radio_present_) {
|
if (radio_present_) {
|
||||||
radio.openReadingPipe(1, RADIO_ADDRESS);
|
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all
|
||||||
radio.setPALevel(RF24_PA_LOW);
|
radio.openReadingPipe(1, HXP_RADIO_ADDRESS);
|
||||||
radio.setDataRate(RF24_250KBPS);
|
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
|
||||||
radio.setChannel(96); // pick one clear of your local Wi-Fi
|
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
|
||||||
|
radio.setChannel(HXP_RADIO_CHANNEL);
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
}
|
}
|
||||||
return radio_present_;
|
return radio_present_;
|
||||||
|
|||||||
@@ -1,26 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
#include <HexapodProtocol.h> // CmdPacket + FLAG_* + radio settings, shared with the remote (shared/)
|
||||||
// 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
|
|
||||||
|
|
||||||
class Comms {
|
class Comms {
|
||||||
public:
|
public:
|
||||||
|
|||||||
@@ -49,9 +49,10 @@ constexpr float SWING_LIFT_MM = 35.0f;
|
|||||||
constexpr float CONTROL_HZ = 60.0f;
|
constexpr float CONTROL_HZ = 60.0f;
|
||||||
|
|
||||||
// ---- Radio (nRF24L01) ----
|
// ---- 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_CE_PIN = 33;
|
||||||
constexpr uint8_t RADIO_CSN_PIN = 34;
|
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;
|
constexpr float LINK_TIMEOUT_SEC = 0.3f;
|
||||||
|
|
||||||
// ---- Setpoint smoothing rates ----
|
// ---- Setpoint smoothing rates ----
|
||||||
|
|||||||
@@ -56,5 +56,15 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
|
|||||||
out.pose.yaw_deg = rz * MAX_TILT_DEG;
|
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;
|
return out;
|
||||||
}
|
}
|
||||||
@@ -1,16 +1,20 @@
|
|||||||
# Hexapod robot firmware (Teensy 4.0)
|
# Hexapod robot firmware (Teensy 4.0)
|
||||||
|
|
||||||
This is the **robot-side** firmware only. It is fully functional on its own
|
This is the **robot-side** firmware. It is fully functional on its own
|
||||||
(driven by the serial test harness below) and is already wired to receive
|
(driven by the serial test harness below) and receives commands from the
|
||||||
commands from a future remote over nRF24L01 — the packet format in
|
remote (`../remote/`) over nRF24L01. The packet format lives in
|
||||||
`Comms.h` is the contract the remote sketch must match.
|
`../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
|
Arduino IDE also works if you copy `shared/HexapodProtocol/` into your
|
||||||
- **Servo** — bundled with Teensyduino, nothing to install
|
sketchbook `libraries/` folder and install **RF24** by TMRh20.
|
||||||
|
|
||||||
## Before you trust this on the floor
|
## 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_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
|
- `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` — calibrate per servo, they vary
|
||||||
batch to batch
|
batch to batch
|
||||||
- `RADIO_ADDRESS` / channel (set in `Comms.cpp`) — must match the remote
|
- `RADIO_CE_PIN` / `RADIO_CSN_PIN` — your nRF24 wiring. Address, channel,
|
||||||
once it exists
|
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)
|
## 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
|
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
|
needs a different, sequenced approach (fold legs up rather than just
|
||||||
lowering).
|
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
|
## Testing without a remote yet
|
||||||
|
|
||||||
@@ -61,8 +77,8 @@ keystrokes) and use:
|
|||||||
|-----|--------|
|
|-----|--------|
|
||||||
| `a` | toggle armed |
|
| `a` | toggle armed |
|
||||||
| `s` | toggle sit / stand |
|
| `s` | toggle sit / stand |
|
||||||
| `w` / `x` | walk forward / backward |
|
| `w` / `x` | walk forward / backward (`vx` ±) |
|
||||||
| `q` / `e` | strafe left / right |
|
| `q` / `e` | strafe left / right (`vy` ±) |
|
||||||
| `z` / `c` | yaw left / right |
|
| `z` / `c` | yaw left / right |
|
||||||
| `g` | toggle gait: tripod ↔ wave |
|
| `g` | toggle gait: tripod ↔ wave |
|
||||||
| `m` | toggle walk mode: direct (A) ↔ heading-hold (B) |
|
| `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
|
enforces `DISARMED → SIT → STAND → WALKING` in order and will otherwise
|
||||||
ignore walk commands.
|
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
|
- `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the
|
||||||
wire format the remote must send (as an RF24 payload to `RADIO_ADDRESS`
|
exact wire format. Both radios set a static payload size of
|
||||||
on the same channel/data-rate/PA-level set in `Comms::begin()`).
|
`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()`
|
- 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.
|
||||||
- `FLAG_REMOTE_GYRO` / `FLAG_ROBOT_GYRO` independently gate whether the
|
- The right stick's roll/pitch/yaw (or X/Y/Z) always apply. The remote's
|
||||||
remote's tilt feeds into body pose and whether the robot's own IMU
|
handset tilt arrives in its own `remote_roll` / `remote_pitch` fields and
|
||||||
stabilizes it — matching the touchscreen toggles you described.
|
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
|
- `FLAG_RSTICK_XLATE` switches the right stick between rotation-axes and
|
||||||
translation-axes interpretation, and `FLAG_MODE_B` switches between the
|
translation-axes interpretation, and `FLAG_MODE_B` switches between the
|
||||||
two walk modes, all inside the existing `InputMapper`.
|
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
|
├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend
|
||||||
├── InputMapper.h/.cpp - CmdPacket -> twist/pose/mode decoding
|
├── InputMapper.h/.cpp - CmdPacket -> twist/pose/mode decoding
|
||||||
├── Limits.h/.cpp - workspace clamp, joint rate limit, stance polygon check
|
├── 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
|
└── ServoDriver.h/.cpp - servo attach/relax/write
|
||||||
```
|
```
|
||||||
@@ -36,10 +36,11 @@ void pollSerialTestInput() {
|
|||||||
switch (c) {
|
switch (c) {
|
||||||
case 'a': test_packet.flags ^= FLAG_ARMED; break;
|
case 'a': test_packet.flags ^= FLAG_ARMED; break;
|
||||||
case 's': test_packet.flags ^= FLAG_STAND; 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;
|
// vx = forward (+X), vy = strafe left (+Y) - see HexapodProtocol.h axis convention.
|
||||||
case 'x': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break;
|
case 'w': test_packet.vx = min(test_packet.vx + 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 'x': 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;
|
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 '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 '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;
|
case 'g': test_packet.flags ^= FLAG_GAIT_WAVE; break;
|
||||||
@@ -108,13 +109,9 @@ void loop() {
|
|||||||
|
|
||||||
gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
|
gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
|
||||||
|
|
||||||
// Remote-gyro toggle off -> drop commanded tilt but keep XYZ offset/height.
|
// Stick pose + remote-tilt feedforward (gated on FLAG_REMOTE_GYRO) are
|
||||||
PoseCommand pose_cmd = mapped.pose;
|
// already combined in InputMapper.
|
||||||
if (!mapped.remote_gyro_enabled) {
|
pose.setCommand(mapped.pose);
|
||||||
pose_cmd.roll_deg = 0;
|
|
||||||
pose_cmd.pitch_deg = 0;
|
|
||||||
}
|
|
||||||
pose.setCommand(pose_cmd);
|
|
||||||
pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0,
|
pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0,
|
||||||
mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0);
|
mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0);
|
||||||
pose.update(dt);
|
pose.update(dt);
|
||||||
|
|||||||
@@ -0,0 +1,59 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <Arduino.h>
|
||||||
|
|
||||||
|
// =====================================================================
|
||||||
|
// 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
|
||||||
@@ -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"
|
||||||
|
}
|
||||||
Reference in new issue
Block a user