Compare commits
22
Commits
91b5b99fd5
..
main
No files matched your search
@@ -38,3 +38,4 @@
|
||||
*.ide-shm filter=lfs diff=lfs merge=lfs -text
|
||||
2018-unity-sim/**/*.unity filter=lfs diff=lfs merge=lfs -text
|
||||
*.bytes filter=lfs diff=lfs merge=lfs -text
|
||||
*.3mf filter=lfs diff=lfs merge=lfs -text
|
||||
@@ -0,0 +1,2 @@
|
||||
# local editor settings
|
||||
.vscode/
|
||||
@@ -0,0 +1,3 @@
|
||||
[submodule "2026-current/tools/nextion-hmi-tools"]
|
||||
path = 2026-current/tools/nextion-hmi-tools
|
||||
url = https://mangavalk.com/mangavalk/openhmi-nx.git
|
||||
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"mcpServers": {
|
||||
"nxhmi": {
|
||||
"command": "python",
|
||||
"args": ["-m", "nxhmi.mcp_server"],
|
||||
"env": {"PYTHONPATH": "2026-current/tools/nextion-hmi-tools/src"}
|
||||
}
|
||||
}
|
||||
}
|
||||
+104
-55
@@ -1,7 +1,7 @@
|
||||
# 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).
|
||||
linked over nRF24L01: commands remote → robot, telemetry back in the radio's ack payloads.
|
||||
A Unity simulation runs the *unchanged* robot firmware as a DLL. This file
|
||||
is the persistent context for working on any part of it. Read it fully
|
||||
before changing anything.
|
||||
@@ -10,11 +10,14 @@ before changing anything.
|
||||
|
||||
```
|
||||
robot/ Teensy 3.6 firmware (PlatformIO env robot)
|
||||
remote/ Teensy 4.1 firmware (PlatformIO env remote)
|
||||
remote/ Teensy 4.0 firmware (PlatformIO env remote)
|
||||
shared/HexapodProtocol/ wire protocol, used by robot, remote and sim
|
||||
sim/ robot firmware -> hexapod_sim.dll (CMake + g++), headless smoke test
|
||||
Hexapod_Simulation_Unity/ Unity 6000.6 project that loads the DLL (Assets/HexapodSim/)
|
||||
scripts/ PlatformIO helper
|
||||
dongle/ USB nRF24 stick firmware (STM32F030): the PC's radio, bridge mode for Unity
|
||||
hardware/ OpenSCAD rebuild + new remote case, Fusion 360 designs, PCB notes
|
||||
scripts/ PlatformIO helper, make_remote_hmi.py (generates remote/remote.HMI)
|
||||
tools/nextion-hmi-tools/ git submodule: nxhmi (.HMI read/write/render, MCP server)
|
||||
```
|
||||
|
||||
This folder is `2026-current/` inside the `hexapod-robot` repo. The older generations sit next to it
|
||||
@@ -34,7 +37,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
| Env | Board | Sketch dir | Purpose |
|
||||
|-----|-------|------------|---------|
|
||||
| `robot` | `teensy36` | `robot/` | walking robot |
|
||||
| `remote` | `teensy41` | `remote/` | handheld transmitter |
|
||||
| `remote` | `teensy40` | `remote/` | handheld transmitter |
|
||||
|
||||
- `shared/` is PlatformIO's `lib_dir`. `shared/HexapodProtocol/` is a
|
||||
header-only library both sketches `#include <HexapodProtocol.h>`.
|
||||
@@ -51,7 +54,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
|
||||
## Communication contract (`shared/HexapodProtocol/HexapodProtocol.h`)
|
||||
|
||||
`CmdPacket`, 22 bytes (static_asserted; protocol v3), sent remote → robot at 100 Hz
|
||||
`CmdPacket`, 26 bytes (static_asserted; protocol v4, `HXP_PROTOCOL_VERSION`), sent remote → robot at 100 Hz
|
||||
(robot `CONTROL_HZ` is 60):
|
||||
|
||||
| Field | Range | Meaning |
|
||||
@@ -65,6 +68,16 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
| `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) |
|
||||
| `cal_cmd` | `CAL_*` | servo centre calibration, only while not armed: `CAL_NONE` normal, `HOLD` centre pose, `SET` trim, `SAVE` / `REVERT` EEPROM. States, not events: repeats are harmless |
|
||||
| `cal_servo`, `cal_value` | 0–17, ±200 µs | selected servo (leg × 3 + joint) and its trim for `CAL_SET` |
|
||||
|
||||
`Telemetry`, 30 bytes, robot → remote as the **ack payload** of the remote's
|
||||
next packet (`Comms::loadAckPayload` after every packet received; no extra
|
||||
transmissions): robot state (`TSTATE_*`, 4 = SERVO_CAL), status bits
|
||||
(`TSTAT_*`: IMU ok, gyro on, servos driven, trims dirty / stored), IMU
|
||||
roll/pitch in 0.1°, battery mV (0 = not measured), the 18 commanded joint
|
||||
angles as int8 (tibia stored + 90), packets received per second, and the
|
||||
echo of `cal_servo` with its trim.
|
||||
|
||||
- **Axis convention** (ROS REP-103 style): body +X forward, +Y left, +Z up.
|
||||
+roll = right side down, +pitch = nose down, +yaw = CCW.
|
||||
@@ -72,11 +85,15 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
`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`.
|
||||
96, 250 kbps, PA LOW. Ack payloads need dynamic payload lengths
|
||||
(`enableAckPayload()` on both radios), so **each end drops any packet
|
||||
whose length isn't sizeof() of its struct**. That keeps version
|
||||
mismatches failing closed: packets are ignored, the robot times out, and
|
||||
it disarms.
|
||||
- **Changing the protocol means reflashing both boards.** Keep both
|
||||
structs ≤ 32 bytes, update the `static_assert`s, bump
|
||||
`HXP_PROTOCOL_VERSION`, and mirror `CmdPacket` in the Unity
|
||||
`HexapodSimNative.cs`.
|
||||
- **Failsafes:** the robot sets `linkOk()` false after `LINK_TIMEOUT_SEC`
|
||||
(0.3 s) without packets and then uses a default `CmdPacket` (flags 0 =
|
||||
disarmed, servos detached). The remote disarms itself after
|
||||
@@ -101,33 +118,41 @@ joint-rate clamp → servos.
|
||||
| `Kinematics` | per-leg 3-DOF analytic IK, `neutralStance()` |
|
||||
| `GaitEngine` | tripod/wave patterns, per-leg phase, pattern swap only at touchdown. `limitTwist()` (called in robot.ino right after InputMapper, so the heading estimate integrates the *achieved* ω) scales the walk command until its stride envelope (`strideEnvelope()`: even/odd legs at opposite stride ends, plus a lifted set) is feasible, so full-rate turning can't make neighbouring legs collide. Stance feet are *integrated* each tick from the current twist (v + ω×r at the foot); swing feet arc from lift-off to the next leading edge. `resetFeet()` on entering WALKING restarts the cycle with grounded legs at mid-stance |
|
||||
| `BodyPose` | 6-axis body pose (smoothed command + IMU roll/pitch correction), applied to foot targets as the *inverse* rotation, so the body moves as commanded. `fitToReach()` scales the *whole* pose by one factor (binary search on `Limits::feetFeasible`; drops at once, recovers at 1/s) so combined poses never bend individual legs out of a rigid foot pattern or into each other. While walking it fits against the stride envelope, not the current feet, so it doesn't saw with the step cycle |
|
||||
| `RobotIMU` | MPU6050 complementary filter (α 0.98), gated by `FLAG_ROBOT_GYRO` |
|
||||
| `RobotIMU` | MPU6050 complementary filter (α 0.98). Always estimates (the remote shows it); only the self-leveling in robot.ino is gated by `FLAG_ROBOT_GYRO` |
|
||||
| `StateMachine` | DISARMED → SIT → STAND → WALKING, sit/stand blend at 1/s. Disarm is instant |
|
||||
| `InputMapper` | CmdPacket → Twist + PoseCommand + mode flags + stance height. Mode B = steering **relative to where the body faces right now**: the forward/back stick component sets walking direction (vx sign), the sideways component steers (target = current heading + atan2(vy, |vx|) every tick). So W = straight on, S = straight **back** (not turn around; owner's request), W/S+A = walk while turning left, A alone = turn on the spot while held. On release the target freezes at the current heading (hold). An earlier version froze the reference at the first push, so heading only changed after releasing W; the owner rejected that. Mode A keeps the target = current heading so switching to B never jumps. Forward speed × max(0, cos(heading error)), so big corrections turn in place first. Adds remote tilt on top of stick pose when `FLAG_REMOTE_GYRO`, clamped to `MAX_TILT_DEG` |
|
||||
| `Limits` | workspace clamp (70 mm horiz / 60 mm vert), joint rate clamp, ≥3-legs-in-stance check (not acted on yet). `feetFeasible()` = every foot in its workspace with valid IK **and** neighbouring legs' segments (coxa/femur/tibia, via `Kinematics::jointPositions`) ≥ `LEG_CLEARANCE_MM` apart. This is the single "physically possible" test used by gait and pose limiting |
|
||||
| `Comms` | nRF24 RX, link timeout, `injectPacket()` for the harness |
|
||||
| `ServoDriver` | 18 servos on hardware PWM (50 Hz, 16-bit), per-servo calibration from `Config.h` `SERVOS`, duty 0 = relaxed, skips invalid IK solutions |
|
||||
| `Comms` | nRF24 RX (drops wrong-length packets), link timeout, telemetry as ack payload, packets/s, `injectPacket()` for the harness |
|
||||
| `ServoDriver` | 18 servos on hardware PWM (50 Hz, 16-bit), per-servo calibration from `Config.h` `SERVOS` plus a per-servo centre trim (µs, EEPROM record at address 0, loaded in setup), duty 0 = relaxed, skips invalid IK solutions. `calibrate()` = SERVO_CAL: applies a `CAL_*` command and holds `CAL_POSE` (coxa 0, femur 0, tibia −90) |
|
||||
| `Battery` | battery voltage through a divider; `BATTERY_SENSE_PIN` 255 = not wired yet |
|
||||
|
||||
## Remote architecture (`remote/`, Teensy 4.1)
|
||||
`robot.ino` also runs SERVO_CAL (disarmed + `cal_cmd != CAL_NONE`; leaving
|
||||
it relaxes the servos) and fills the `Telemetry` every loop
|
||||
(`sendTelemetry`).
|
||||
|
||||
## Remote architecture (`remote/`, Teensy 4.0)
|
||||
|
||||
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.
|
||||
tick: sticks → IMU → radio link status → battery → RemoteState (smoothers,
|
||||
link-loss disarm, servo-trim read-back from telemetry) → PacketBuilder →
|
||||
`radio.write` (+ read the ack payload's telemetry) → 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) |
|
||||
| `UiTypes.h` | `UiCmd`/`UiEvent` (operator requests), `UiPage` (HMI page ids), `UiView` (what the display shows, incl. robot telemetry) |
|
||||
| `Sticks` | 6 analog axes: ADC averaging + IIR, piecewise calibration around center, invert, deadzone with rescale, expo → ±1000. Boot-time center capture, sweep calibration saved to EEPROM (magic/version/checksum) |
|
||||
| `RemoteIMU` | MPU6050, same complementary filter as the robot, axis remap for mounting, boot gyro-bias capture, "level" zero |
|
||||
| `RemoteState` | authoritative operator state and interlocks: arm needs centered sticks, disarm clears stand, stand needs armed, calibrate needs disarmed, link-loss auto-disarm. Smooths speed scale and fades remote tilt in/out |
|
||||
| `RemoteState` | authoritative operator state and interlocks: arm needs centered sticks, disarm clears stand, stand needs armed, calibrate needs disarmed, link-loss auto-disarm. Smooths speed scale and fades remote tilt in/out. Servo calibration: only while disarmed and on the CAL page; a newly selected servo is `CAL_HOLD` until its trim has been read back from telemetry, then `CAL_SET`; SAVE/REVERT go out for 300 ms. UPDATE DISPLAY needs a second tap |
|
||||
| `PacketBuilder` | the inverse of InputMapper: stick frame → body frame per the contract, per-submode right-stick mapping, `FLAG_WALK` with 250 ms linger (twist ignored in mode B), tilt deadzone/scale × fade, zeros motion when disarmed |
|
||||
| `RadioLink` | nRF24 TX, short retries (1, 3), ack-based link status and 32-packet quality % |
|
||||
| `NextionUI` | library-free Nextion driver on `Serial1`. HMI sends `'#' len cmd [val]` frames via `printh`. Remote pushes state by component name, as `control.<name>` (the owner adds a `control` page with vscope-global components to the old HMI), with diffing, a 2 s full refresh, and a redraw on the Nextion 0x88 "ready" frame. Also writes the old HMI's `t1` status texts (`NEXTION_STATUS_TEXTS`). The old page-1 `b2` sends a standard 0x65 touch frame, which triggers `runUploadPassthrough()`: USB ↔ display bridge for Nextion Editor, following `Serial.baud()`, never returns. Serial key `U` does the same |
|
||||
| `RadioLink` | nRF24 TX, short retries (1, 3), ack-based link status and 32-packet quality %, telemetry from the ack payloads (stale after `TELEMETRY_STALE_MS`) |
|
||||
| `Battery` | handset battery through a divider; `BATTERY_SENSE_PIN` 255 = not wired yet (planned A6) |
|
||||
| `NextionUI` | library-free Nextion driver on `Serial2` (pins 7/8). HMI sends `'#' len cmd [val]` frames via `printh`; every page's Preinitialize sends `'P'` + page id (and `sendme` / 0x66 recovers the page after a remote restart). The remote writes **only the showing page**, by local component name, and only what changed (FNV hash per instruction in a per-page slot cache), 20 Hz (SERVOS 10 Hz), never blocking on a full TX buffer; full refresh every 2 s and on the 0x88 "ready" frame. UPDATE DISPLAY (tapped twice) or serial `U` runs `runUploadPassthrough()`: USB ↔ display bridge for Nextion Editor, following `Serial.baud()`, never returns |
|
||||
| `SerialConsole` | USB keystrokes that mirror every UI command plus diagnostics |
|
||||
| `SmoothedValue.h` | copy of the robot's (kept separate so robot/ stays untouched) |
|
||||
| `NEXTION.md` | HMI build guide: component names, txt_maxl, touch event code |
|
||||
| `NEXTION.md` | the HMI: pages, command bytes, which components the firmware writes |
|
||||
| `remote.HMI` | **generated** by `scripts/make_remote_hmi.py` (pages splash, drive, status, servos, cal, setup, upload; tabs + top bar with DISARM). Change the layout in the script, not in the Editor |
|
||||
|
||||
Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
= CCW. The mapping into the packet is in `PacketBuilder.cpp` (e.g.
|
||||
@@ -166,8 +191,10 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
`Knie.x = −(tibia + 90)`.
|
||||
- **Unity (`Assets/HexapodSim/`):**
|
||||
- `HexapodSimBootstrap` auto-adds `HexapodSimulation` in any scene and
|
||||
disables the old scripts (`Walk`, `RotationControl`, `Leg`,
|
||||
`HandleHoystick`). Those are kept for reference only.
|
||||
disables the old scripts (`Walk`, `RotationControl`, `Leg`). Those are
|
||||
kept for reference only. The old USB serial / joystick scripts
|
||||
(`Serial*`, `HandleHoystick`) were removed (owner, 2026-11-17: no USB
|
||||
links to remote or robot any more, everything goes over the radio).
|
||||
- `OpenScadModel` replaces the old scene's robot with the OpenSCAD model
|
||||
(meshes from `hardware/openscad/export_unity.py` in
|
||||
`Resources/OpenScadModel/`): it builds a new rig with the same names at
|
||||
@@ -176,9 +203,16 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
- `HexapodSimNative` loads a temp copy of the DLL via
|
||||
LoadLibrary/GetProcAddress, so there's no file lock and each Play is a
|
||||
fresh power-on.
|
||||
- `SimRemote` stands in for the remote (Joystick axes "1"–"6" as in the
|
||||
old project, Keyboard, or the robot's own serial harness) and mirrors
|
||||
`remote/PacketBuilder.cpp` + `RemoteState.cpp` rules. Keep them in sync.
|
||||
- `SimRemote` has two sources. **Keyboard** stands in for the remote and
|
||||
mirrors `remote/PacketBuilder.cpp` + `RemoteState.cpp` rules (keep them
|
||||
in sync). **Real remote** takes the real remote's packets over the air
|
||||
from the USB nRF24 dongle (`DongleLink.cs`, the dongle's bridge mode,
|
||||
see `dongle/README.md`), feeds them to the firmware unchanged
|
||||
(`HexapodSimNative.RadioSend(byte[])`) and returns the robot's
|
||||
telemetry as the ack payload, so the remote's display shows the sim.
|
||||
`DongleLink` opens the port through kernel32 (no `System.IO.Ports`),
|
||||
auto-finds the CH340 (VID 1A86 PID 7523), and does all port I/O on one
|
||||
background thread.
|
||||
- `SimTestPanel` is the IMGUI panel (F1).
|
||||
- Body motion is kinematic and lives **in the DLL** (`sim/SimBody.cpp`,
|
||||
`hxsim_get_body`), so Unity shows exactly what `sim_smoke` checks.
|
||||
@@ -203,8 +237,11 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
stand → walk. The harness does not exercise the right stick or remote
|
||||
tilt. Add keys if needed.
|
||||
- **Robot firmware in simulation:** `sim/build/sim_smoke.exe` (headless,
|
||||
scripted checks), or Unity Play mode with Keyboard / Joystick / Robot
|
||||
serial input.
|
||||
scripted checks), or Unity Play mode with Keyboard input or the real
|
||||
remote over the radio (USB nRF24 dongle).
|
||||
- **Remote without robot:** the Unity sim behind the USB nRF24 dongle
|
||||
answers like the robot (telemetry, servo calibration), see
|
||||
`sim/README.md`.
|
||||
- **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
|
||||
@@ -235,7 +272,7 @@ Robot (`robot/README.md` has detail):
|
||||
limits now in `Config.h`: coxa limits are **per leg**
|
||||
(`COXA_MIN_DEG[i]`/`COXA_MAX_DEG[i]`; all ±60 in the hexagon layout, but in
|
||||
the old sideways layout a corner leg only swung ~36° towards its mid leg),
|
||||
and `LEG_CLEARANCE_MM` is 56 (a leg is 55.2 mm wide at the joints). The sim API exports the per-leg coxa limits (version 7).
|
||||
and `LEG_CLEARANCE_MM` is 56 (a leg is 55.2 mm wide at the joints). The sim API exports the per-leg coxa limits (version 7). Version 8 adds `hxsim_get_ack_payload` (the robot's telemetry); sim_smoke checks telemetry and every `CAL_*` command.
|
||||
- **Fixed (owner-approved):** the `Kinematics.cpp` femur elevation sign
|
||||
(`atan2(local_z, horiz)`; it used to solve for a foot above the hip, so
|
||||
every solution was invalid). Stance pose is now femur −1.2°, tibia
|
||||
@@ -365,22 +402,35 @@ Robot (`robot/README.md` has detail):
|
||||
|
||||
Remote (`remote/README.md` has detail):
|
||||
- Pins in `remote/Config.h`, taken from the old remote sketch: radio CE 9 /
|
||||
CSN 10, sticks A0–A5, Nextion on Serial1 (0/1). The MPU6050 is on
|
||||
**Wire2 (25/24)**, since A4/A5 = Wire's SDA/SCL are stick pins; that's
|
||||
the owner's choice and needs rewiring the MPU. Which stick axis is on
|
||||
which pin is still unknown.
|
||||
CSN 10, Nextion on **Serial2 (7/8)**, confirmed on the real remote. Owner's choice
|
||||
(2026-11-17): edge headers only, no pads underneath. So the MPU6050 is on **Wire (18/19)**
|
||||
and the two stick wires that were on A4/A5 (18/19) move to **A7/A8 (21/22)**: sticks
|
||||
`{ A0, A1, A2, A3, A7, A8 }`. A6 (20) is kept for the battery divider. Checked on the real
|
||||
remote (2026-11-17): left stick X/Y/twist = A3/A7/A8, right = A0/A1/A2, all inverted but the
|
||||
right twist; each stick only moves about ±700 counts from centre, so run the sweep
|
||||
calibration. The IMU axis defaults are right. Its MPU6050 (likely a clone) reads all zeros
|
||||
until it gets a full device reset, so `RemoteIMU::begin()` resets it first. `RobotIMU` only
|
||||
wakes its chip (owner-verified code, not changed): check the robot's IMU once it's flashed.
|
||||
- `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`. The owner's old 6-page HMI is in `../2019-teensy-nrf24/remote-nextion/remote.HMI`;
|
||||
its component names aren't readable outside Nextion Editor. The compiled
|
||||
`.tft` is gitignored.
|
||||
`NEXTION.md`. The working HMI is `remote/remote.HMI`, generated by
|
||||
`scripts/make_remote_hmi.py` (owner-approved design, 2026-11-17; the old 6-page one in
|
||||
`../2019-teensy-nrf24/remote-nextion/` only lends its splash picture). It's on the real
|
||||
display (turned 180°, see `remote/NEXTION.md`). Edit and render it with the `nxhmi` MCP server
|
||||
(registered in the repo root `.mcp.json`; code is the git submodule `tools/nextion-hmi-tools`,
|
||||
run `git submodule update --init` after cloning; CLI `python -m nxhmi` with `PYTHONPATH` =
|
||||
`tools/nextion-hmi-tools/src`): `info`, `get_page`, `render_page`, `set_attributes`,
|
||||
`set_event`, `add_component`, `add_page`, `verify`, `build_tft`. Its `.tft` builder is
|
||||
partial and needs the Nextion firmware files, which aren't in the submodule (they're kept
|
||||
locally in `D:\Claude\nextion-hmi-tools\firmware`), so compile the upload `.tft` in Nextion
|
||||
Editor. The compiled `.tft` is gitignored.
|
||||
- What the old remote sketch (`../2019-teensy-nrf24/remote/`, last ran on a
|
||||
Teensy) tells us about the real hardware:
|
||||
- **Nextion on `Serial1` (pins 0/1)**, driven by the ITEAD Nextion
|
||||
library, whose `nexInit()` runs at 9600. `remote/Config.h` now uses
|
||||
Serial1 and expects 115200 via `bauds=` in the HMI.
|
||||
- **Nextion on `Serial2` (pins 7/8)**, driven by the ITEAD Nextion
|
||||
library (its `nexSerial` is Serial2). The sketch's `Serial1.begin()` and
|
||||
its USB ↔ Serial1 passthrough never reached the display: a probe on the
|
||||
real remote (2026-11-17) got `comok` only on Serial2, at 115200.
|
||||
`remote/Config.h` uses Serial2 at 115200 (`bauds=` in the HMI).
|
||||
- The old HMI is a *servo monitor*, not the control UI in `NEXTION.md`:
|
||||
- page 1: `t1` "connected"/"not connected", and `b2` = enter
|
||||
upload mode;
|
||||
@@ -388,15 +438,16 @@ Remote (`remote/README.md` has detail):
|
||||
`t1` status;
|
||||
- page 4: `uploadscreen`.
|
||||
|
||||
None of the arm/stand/gait/mode/gyro/speed/height controls exist yet.
|
||||
The owner chose to add a `control` page per `NEXTION.md`.
|
||||
It had none of the arm/stand/gait/mode/gyro/speed/height controls.
|
||||
It was replaced by the generated tabbed HMI (see above).
|
||||
- `b2` → **Nextion upload passthrough**: shows `uploadscreen`, then
|
||||
bridges USB ↔ Serial1 forever, so Nextion Editor can upload a `.tft`
|
||||
through the remote's USB port. The new firmware keeps this.
|
||||
- Sticks on **A0–A5**, contiguous (`analogInPin + i`). On the Teensy 4.1,
|
||||
bridges USB ↔ the display serial forever, so Nextion Editor can upload a `.tft`
|
||||
through the remote's USB port. The new firmware keeps this as SETUP ›
|
||||
UPDATE DISPLAY (page `upload`).
|
||||
- Sticks on **A0–A5**, contiguous (`analogInPin + i`). On the Teensy 4.0,
|
||||
A4/A5 are pins 18/19 = Wire SDA/SCL, which the MPU6050 needs. The old
|
||||
code had the MPU code disabled (`if (false)`). Owner's decision: keep
|
||||
the sticks, move the MPU to Wire2.
|
||||
code had the MPU code disabled (`if (false)`). Owner's decision: MPU on
|
||||
Wire (18/19), those two stick wires move to A7/A8 (21/22).
|
||||
- nRF24 on CE 9 / CSN 10, the same as `remote/Config.h`. It used
|
||||
RF24Network on channel 90, which the new protocol doesn't.
|
||||
- Stick calibration was a 10 s min/max sweep saved to EEPROM, output
|
||||
@@ -404,9 +455,6 @@ Remote (`remote/README.md` has detail):
|
||||
- USB serial at 250000. It received 18 servo bytes + checksum + `0xFE`
|
||||
from the old Unity `Serial.cs` (servo passthrough, forwarded over RF
|
||||
to the robot) and printed `i_value,...` stick lines.
|
||||
- The Unity remote stand-in's joystick axis assignment (which of axes 4/6
|
||||
is forward) couldn't be recovered from the old code. Check it with the
|
||||
panel's raw-axis readout.
|
||||
- Radio retry timing (`RADIO_RETRY_*`) is untested under real link loss.
|
||||
|
||||
## Working rules for this repo
|
||||
@@ -424,7 +472,8 @@ Remote (`remote/README.md` has detail):
|
||||
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/`.
|
||||
- Planned or possible next steps: wire the battery dividers (firmware is
|
||||
ready on both sides, pins still 255) and add a low-battery warning;
|
||||
flash the robot with protocol v4 and try telemetry and servo calibration
|
||||
with it (the real remote ↔ dongle ↔ Unity sim path already works over the
|
||||
air, 2026-11-17); Unity driving the real robot through the dongle.
|
||||
@@ -0,0 +1,256 @@
|
||||
using System;
|
||||
using System.Collections.Concurrent;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// The USB nRF24 dongle (2026-current/dongle/) in bridge mode: it stands in for
|
||||
// the robot's radio. Every packet the real remote sends comes in here; the
|
||||
// simulated robot's Telemetry goes back out as the radio's ack payload, so the
|
||||
// remote and its display see the simulation exactly like the real robot.
|
||||
//
|
||||
// Wire format both ways: A5 type len payload[len] sum (sum = type+len+payload).
|
||||
// to dongle 'L' listen (bridge on), 'X' stop, 'A' ack payload (<= 32 B)
|
||||
// from dongle 'R' received packet, 'S' status (packets u16, acks loaded u16)
|
||||
// The dongle drops the ack payload if it isn't refreshed for ~0.5 s.
|
||||
//
|
||||
// Windows only (the dev PC): the port is opened through kernel32, so there's
|
||||
// no dependency on System.IO.Ports or the project's API compatibility level.
|
||||
public sealed class DongleLink : IDisposable
|
||||
{
|
||||
public string PortName { get; private set; }
|
||||
public bool IsOpen => handle_ != IntPtr.Zero && handle_ != InvalidHandle;
|
||||
public string Error { get; private set; }
|
||||
public int DonglePacketsPerSec { get; private set; } // from the dongle's status frame
|
||||
public int DongleAcksPerSec { get; private set; }
|
||||
public int BadFrames { get; private set; }
|
||||
|
||||
readonly ConcurrentQueue<byte[]> packets_ = new ConcurrentQueue<byte[]>();
|
||||
byte[] pendingAck_; // newest telemetry frame, written by the I/O thread
|
||||
IntPtr handle_ = IntPtr.Zero;
|
||||
Thread io_;
|
||||
volatile bool running_;
|
||||
|
||||
// ---- Opening ----
|
||||
|
||||
// "auto" = the first USB-serial CH340 port Windows knows about (the dongle's
|
||||
// CH330N shows up as VID 1A86 / PID 7523); otherwise e.g. "COM7".
|
||||
public static DongleLink Open(string port)
|
||||
{
|
||||
var link = new DongleLink();
|
||||
string name = port;
|
||||
if (string.IsNullOrEmpty(name) || name.Equals("auto", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
name = FindCh340Port();
|
||||
if (name == null) { link.Error = "no USB nRF24 dongle (CH340, VID 1A86 PID 7523) found"; return link; }
|
||||
}
|
||||
link.PortName = name;
|
||||
link.OpenPort(name);
|
||||
if (link.IsOpen)
|
||||
{
|
||||
link.Write(Frame((byte)'L', null, 0));
|
||||
link.running_ = true;
|
||||
link.io_ = new Thread(link.IoLoop) { IsBackground = true, Name = "DongleLink" };
|
||||
link.io_.Start();
|
||||
}
|
||||
return link;
|
||||
}
|
||||
|
||||
void OpenPort(string name)
|
||||
{
|
||||
// Retry briefly: SignalRGB and similar tools grab CH340 ports on enumeration.
|
||||
for (int attempt = 0; attempt < 25; attempt++)
|
||||
{
|
||||
handle_ = CreateFile(@"\\.\" + name, GENERIC_READ | GENERIC_WRITE, 0, IntPtr.Zero, OPEN_EXISTING, 0, IntPtr.Zero);
|
||||
if (handle_ != InvalidHandle) break;
|
||||
Thread.Sleep(20);
|
||||
}
|
||||
if (handle_ == InvalidHandle)
|
||||
{
|
||||
Error = $"can't open {name} (Win32 error {Marshal.GetLastWin32Error()}; is SignalRGB or a serial monitor holding it?)";
|
||||
handle_ = IntPtr.Zero;
|
||||
return;
|
||||
}
|
||||
var dcb = new DCB { DCBlength = (uint)Marshal.SizeOf<DCB>() };
|
||||
GetCommState(handle_, ref dcb);
|
||||
dcb.BaudRate = 115200;
|
||||
dcb.ByteSize = 8;
|
||||
dcb.Parity = 0;
|
||||
dcb.StopBits = 0;
|
||||
dcb.Flags = 0x0001 | 0x0010; // fBinary, fDtrControl = DTR_CONTROL_ENABLE
|
||||
SetCommState(handle_, ref dcb);
|
||||
// Reads return within ~3 ms, so queued telemetry goes out promptly
|
||||
// (reads and writes on one handle take turns: both run on the I/O thread).
|
||||
var to = new COMMTIMEOUTS { ReadIntervalTimeout = 1, ReadTotalTimeoutConstant = 3, WriteTotalTimeoutConstant = 200 };
|
||||
SetCommTimeouts(handle_, ref to);
|
||||
PurgeComm(handle_, 0x0004 | 0x0008); // RXCLEAR | TXCLEAR
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
running_ = false;
|
||||
io_?.Join(300);
|
||||
if (IsOpen)
|
||||
{
|
||||
Write(Frame((byte)'X', null, 0)); // bridge off: the remote sees the link drop
|
||||
CloseHandle(handle_);
|
||||
}
|
||||
handle_ = IntPtr.Zero;
|
||||
}
|
||||
|
||||
// ---- Traffic ----
|
||||
|
||||
public bool TryTakePacket(out byte[] packet) => packets_.TryDequeue(out packet);
|
||||
|
||||
// Queues the robot's telemetry for the ack of the remote's next packet.
|
||||
// Only the newest one matters, so an unsent older one is replaced.
|
||||
public void SendAckPayload(byte[] data, int len)
|
||||
{
|
||||
if (len <= 32) Volatile.Write(ref pendingAck_, Frame((byte)'A', data, len));
|
||||
}
|
||||
|
||||
static byte[] Frame(byte type, byte[] data, int len)
|
||||
{
|
||||
var f = new byte[4 + len];
|
||||
f[0] = 0xA5; f[1] = type; f[2] = (byte)len;
|
||||
int sum = type + len;
|
||||
for (int i = 0; i < len; i++) { f[3 + i] = data[i]; sum += data[i]; }
|
||||
f[3 + len] = (byte)sum;
|
||||
return f;
|
||||
}
|
||||
|
||||
bool Write(byte[] f)
|
||||
{
|
||||
if (WriteFile(handle_, f, (uint)f.Length, out _, IntPtr.Zero)) return true;
|
||||
Error = $"write failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
|
||||
return false;
|
||||
}
|
||||
|
||||
void IoLoop()
|
||||
{
|
||||
var chunk = new byte[512];
|
||||
var buf = new byte[4096];
|
||||
int have = 0;
|
||||
while (running_)
|
||||
{
|
||||
byte[] ack = Interlocked.Exchange(ref pendingAck_, null);
|
||||
if (ack != null && !Write(ack)) break;
|
||||
if (!ReadFile(handle_, chunk, (uint)chunk.Length, out uint got, IntPtr.Zero))
|
||||
{
|
||||
if (running_) Error = $"read failed (Win32 error {Marshal.GetLastWin32Error()}): dongle unplugged?";
|
||||
break;
|
||||
}
|
||||
if (got == 0) continue;
|
||||
if (have + got > buf.Length) have = 0; // garbage flood: start over
|
||||
Buffer.BlockCopy(chunk, 0, buf, have, (int)got);
|
||||
have += (int)got;
|
||||
have = ParseFrames(buf, have);
|
||||
}
|
||||
}
|
||||
|
||||
// Consumes complete frames from buf[0..len), returns how many bytes remain.
|
||||
int ParseFrames(byte[] buf, int len)
|
||||
{
|
||||
int i = 0;
|
||||
while (true)
|
||||
{
|
||||
while (i < len && buf[i] != 0xA5) i++; // skip text between frames
|
||||
if (len - i < 4) break;
|
||||
byte type = buf[i + 1], n = buf[i + 2];
|
||||
if (n > 32) { i++; continue; }
|
||||
if (len - i < 4 + n) break;
|
||||
int sum = type + n;
|
||||
for (int k = 0; k < n; k++) sum += buf[i + 3 + k];
|
||||
if ((byte)sum != buf[i + 3 + n]) { BadFrames++; i++; continue; }
|
||||
var payload = new byte[n];
|
||||
Buffer.BlockCopy(buf, i + 3, payload, 0, n);
|
||||
i += 4 + n;
|
||||
if (type == 'R') packets_.Enqueue(payload);
|
||||
else if (type == 'S' && n >= 4)
|
||||
{
|
||||
DonglePacketsPerSec = payload[0] | payload[1] << 8;
|
||||
DongleAcksPerSec = payload[2] | payload[3] << 8;
|
||||
}
|
||||
}
|
||||
int rest = len - i;
|
||||
if (rest > 0 && i > 0) Buffer.BlockCopy(buf, i, buf, 0, rest);
|
||||
return rest;
|
||||
}
|
||||
|
||||
// ---- Finding the port: HKLM\SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523\<inst>\Device Parameters\PortName ----
|
||||
|
||||
static string FindCh340Port()
|
||||
{
|
||||
const string baseKey = @"SYSTEM\CurrentControlSet\Enum\USB\VID_1A86&PID_7523";
|
||||
if (RegOpenKeyEx(HKEY_LOCAL_MACHINE, baseKey, 0, KEY_READ, out IntPtr key) != 0) return null;
|
||||
try
|
||||
{
|
||||
var sb = new StringBuilder(256);
|
||||
for (uint idx = 0; ; idx++)
|
||||
{
|
||||
uint len = (uint)sb.Capacity;
|
||||
if (RegEnumKeyEx(key, idx, sb, ref len, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero) != 0) break;
|
||||
string port = ReadString(HKEY_LOCAL_MACHINE, baseKey + "\\" + sb + "\\Device Parameters", "PortName");
|
||||
if (port != null && PortExists(port)) return port;
|
||||
}
|
||||
}
|
||||
finally { RegCloseKey(key); }
|
||||
return null;
|
||||
}
|
||||
|
||||
static string ReadString(IntPtr root, string path, string value)
|
||||
{
|
||||
var data = new StringBuilder(64);
|
||||
uint size = (uint)(data.Capacity * 2);
|
||||
return RegGetValue(root, path, value, RRF_RT_REG_SZ, IntPtr.Zero, data, ref size) == 0 ? data.ToString() : null;
|
||||
}
|
||||
|
||||
static bool PortExists(string name) => QueryDosDevice(name, new StringBuilder(256), 256) != 0;
|
||||
|
||||
// ---- Win32 ----
|
||||
|
||||
static readonly IntPtr InvalidHandle = new IntPtr(-1);
|
||||
static readonly IntPtr HKEY_LOCAL_MACHINE = new IntPtr(unchecked((int)0x80000002));
|
||||
const uint GENERIC_READ = 0x80000000, GENERIC_WRITE = 0x40000000, OPEN_EXISTING = 3;
|
||||
const int KEY_READ = 0x20019;
|
||||
const uint RRF_RT_REG_SZ = 0x00000002;
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
struct DCB
|
||||
{
|
||||
public uint DCBlength, BaudRate, Flags;
|
||||
public ushort wReserved, XonLim, XoffLim;
|
||||
public byte ByteSize, Parity, StopBits;
|
||||
public sbyte XonChar, XoffChar, ErrorChar, EofChar, EvtChar;
|
||||
public ushort wReserved1;
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Sequential)]
|
||||
struct COMMTIMEOUTS
|
||||
{
|
||||
public uint ReadIntervalTimeout, ReadTotalTimeoutMultiplier, ReadTotalTimeoutConstant,
|
||||
WriteTotalTimeoutMultiplier, WriteTotalTimeoutConstant;
|
||||
}
|
||||
|
||||
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern IntPtr CreateFile(string name, uint access, uint share, IntPtr sec, uint disposition, uint flags, IntPtr template);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool CloseHandle(IntPtr h);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool GetCommState(IntPtr h, ref DCB dcb);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommState(IntPtr h, ref DCB dcb);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool SetCommTimeouts(IntPtr h, ref COMMTIMEOUTS t);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool PurgeComm(IntPtr h, uint flags);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool ReadFile(IntPtr h, byte[] buf, uint n, out uint got, IntPtr ov);
|
||||
[DllImport("kernel32.dll", SetLastError = true)] static extern bool WriteFile(IntPtr h, byte[] buf, uint n, out uint put, IntPtr ov);
|
||||
[DllImport("kernel32.dll", SetLastError = true, CharSet = CharSet.Unicode)]
|
||||
static extern uint QueryDosDevice(string device, StringBuilder target, int max);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegOpenKeyEx(IntPtr key, string sub, int options, int sam, out IntPtr result);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegEnumKeyEx(IntPtr key, uint index, StringBuilder name, ref uint nameLen, IntPtr reserved, IntPtr cls, IntPtr clsLen, IntPtr lastWrite);
|
||||
[DllImport("advapi32.dll", CharSet = CharSet.Unicode)]
|
||||
static extern int RegGetValue(IntPtr key, string sub, string value, uint flags, IntPtr type, StringBuilder data, ref uint size);
|
||||
[DllImport("advapi32.dll")] static extern int RegCloseKey(IntPtr key);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,2 @@
|
||||
fileFormatVersion: 2
|
||||
guid: d16e6c95081705a4788b93525709e796
|
||||
@@ -14,7 +14,10 @@ namespace HexapodSim
|
||||
public short rx, ry, rz;
|
||||
public short remote_roll, remote_pitch;
|
||||
public short body_height; // -1000 lowest .. 0 default .. 1000 highest
|
||||
public short cal_value; // servo centre calibration (remote only; 0 here)
|
||||
public byte speed_scale, flags, seq;
|
||||
public byte cal_cmd; // CAL_NONE (0): the sim's remote never calibrates servos
|
||||
public byte cal_servo;
|
||||
|
||||
public const byte FLAG_ARMED = 1 << 0;
|
||||
public const byte FLAG_STAND = 1 << 1;
|
||||
@@ -73,7 +76,7 @@ namespace HexapodSim
|
||||
// "Reload firmware" and the new code runs. Each load is a fresh power-on.
|
||||
public sealed class HexapodSimNative : IDisposable
|
||||
{
|
||||
public const int ExpectedApiVersion = 7; // HXSIM_API_VERSION
|
||||
public const int ExpectedApiVersion = 8; // HXSIM_API_VERSION
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
|
||||
@@ -88,6 +91,7 @@ namespace HexapodSim
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void ServosFn(int[] us, int[] attached, uint[] writes);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void FloatsFn(float[] values);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void TwoFloatsFn(float a, float b);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int AckFn(byte[] bytes, int cap);
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void BodyFn(float[] pos3, float[] quatWxyz4, out float yawDeg, out int contactMask);
|
||||
|
||||
IntPtr handle_;
|
||||
@@ -99,14 +103,15 @@ namespace HexapodSim
|
||||
VoidFn start_;
|
||||
UIntArgFn step_;
|
||||
PacketFn radioSend_;
|
||||
BytesInFn radioSendBytes_;
|
||||
ImuFn setImu_;
|
||||
BytesInFn serialInput_;
|
||||
BytesOutFn serialOutput_;
|
||||
ServosFn getServos_;
|
||||
FloatsFn getJoints_, getFeet_;
|
||||
StatusFn getStatus_;
|
||||
TwoFloatsFn setGroundTilt_;
|
||||
BodyFn getBody_;
|
||||
AckFn getAckPayload_;
|
||||
|
||||
public string LoadedFrom { get; private set; }
|
||||
|
||||
@@ -137,8 +142,8 @@ namespace HexapodSim
|
||||
n.start_ = n.Fn<VoidFn>("hxsim_start");
|
||||
n.step_ = n.Fn<UIntArgFn>("hxsim_step");
|
||||
n.radioSend_ = n.Fn<PacketFn>("hxsim_radio_send");
|
||||
n.radioSendBytes_ = n.Fn<BytesInFn>("hxsim_radio_send");
|
||||
n.setImu_ = n.Fn<ImuFn>("hxsim_set_imu");
|
||||
n.serialInput_ = n.Fn<BytesInFn>("hxsim_serial_input");
|
||||
n.serialOutput_ = n.Fn<BytesOutFn>("hxsim_serial_output");
|
||||
n.getServos_ = n.Fn<ServosFn>("hxsim_get_servos");
|
||||
n.getJoints_ = n.Fn<FloatsFn>("hxsim_get_joints_deg");
|
||||
@@ -146,6 +151,7 @@ namespace HexapodSim
|
||||
n.getStatus_ = n.Fn<StatusFn>("hxsim_get_status");
|
||||
n.setGroundTilt_ = n.Fn<TwoFloatsFn>("hxsim_set_ground_tilt");
|
||||
n.getBody_ = n.Fn<BodyFn>("hxsim_get_body");
|
||||
n.getAckPayload_ = n.Fn<AckFn>("hxsim_get_ack_payload");
|
||||
|
||||
int version = n.apiVersion_();
|
||||
if (version != ExpectedApiVersion)
|
||||
@@ -178,16 +184,24 @@ namespace HexapodSim
|
||||
public void Start() => start_();
|
||||
public void Step(uint dtMicros) => step_(dtMicros);
|
||||
public void RadioSend(CmdPacket p) => radioSend_(ref p, Marshal.SizeOf<CmdPacket>());
|
||||
// A packet exactly as it came over the air (the real remote, via the dongle).
|
||||
// Any length: the firmware itself drops ones that aren't sizeof(CmdPacket).
|
||||
public void RadioSend(byte[] bytes) => radioSendBytes_(bytes, bytes.Length);
|
||||
|
||||
// Decodes on-air bytes for display; false if the length doesn't match.
|
||||
public static bool TryDecode(byte[] bytes, out CmdPacket p)
|
||||
{
|
||||
p = default;
|
||||
if (bytes == null || bytes.Length != Marshal.SizeOf<CmdPacket>()) return false;
|
||||
var h = GCHandle.Alloc(bytes, GCHandleType.Pinned);
|
||||
try { p = Marshal.PtrToStructure<CmdPacket>(h.AddrOfPinnedObject()); }
|
||||
finally { h.Free(); }
|
||||
return true;
|
||||
}
|
||||
// Overrides the body-driven IMU with fixed readings from then on (normally unused).
|
||||
public void SetImu(Vector3 accelG, Vector3 gyroDps) =>
|
||||
setImu_(accelG.x, accelG.y, accelG.z, gyroDps.x, gyroDps.y, gyroDps.z);
|
||||
|
||||
public void SerialInput(string text)
|
||||
{
|
||||
if (string.IsNullOrEmpty(text)) return;
|
||||
byte[] b = System.Text.Encoding.ASCII.GetBytes(text);
|
||||
serialInput_(b, b.Length);
|
||||
}
|
||||
|
||||
readonly byte[] serialBuf_ = new byte[4096];
|
||||
public string SerialOutput()
|
||||
@@ -205,6 +219,10 @@ namespace HexapodSim
|
||||
readonly float[] bodyPos_ = new float[3], bodyQuat_ = new float[4];
|
||||
// Kinematic body from the DLL, robot-world frame. The rotation is a
|
||||
// robot-frame quaternion held in a Unity Quaternion for its math only.
|
||||
// The robot's telemetry (HexapodProtocol.h Telemetry, 30 bytes) from the
|
||||
// last ack payload; returns its length, 0 before the first packet.
|
||||
public int GetAckPayload(byte[] buf) => getAckPayload_(buf, buf.Length);
|
||||
|
||||
public void GetBody(out Vector3 posMm, out Quaternion rotRobot, out float yawDeg, out int contactMask)
|
||||
{
|
||||
getBody_(bodyPos_, bodyQuat_, out yawDeg, out contactMask);
|
||||
|
||||
@@ -81,7 +81,7 @@ namespace HexapodSim
|
||||
StringBuilder serialLine_ = new StringBuilder();
|
||||
|
||||
static readonly string[] LegacyMovementScripts =
|
||||
{ "Walk", "RotationControl", "Leg", "HandleHoystick", "Serial2", "Serial3" };
|
||||
{ "Walk", "RotationControl", "Leg" };
|
||||
|
||||
// Robot-frame axes, for readability (Unity's Vector3.up etc. are Unity axes).
|
||||
static readonly Vector3 AxisX = new Vector3(1, 0, 0); // forward
|
||||
|
||||
@@ -1,47 +1,28 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
|
||||
namespace HexapodSim
|
||||
{
|
||||
// Stand-in for the handheld remote. Produces CmdPackets exactly per the
|
||||
// protocol contract and sends them over the simulated radio. Toggles and
|
||||
// interlocks mirror remote/RemoteState.cpp and remote/PacketBuilder.cpp -
|
||||
// keep them in sync if those change.
|
||||
//
|
||||
// Input sources:
|
||||
// Joystick - the real remote as a USB game controller, read through
|
||||
// Unity input axes "1".."6" like the old HandleHoystick.
|
||||
// Keyboard - test without any remote (keys listed in SimTestPanel).
|
||||
// RobotSerialHarness - no radio at all; typed keys go to the firmware's own
|
||||
// TEST_SERIAL_INPUT harness, like the Arduino Serial Monitor.
|
||||
// Where the simulated robot's commands come from. Two sources:
|
||||
// Keyboard - a stand-in remote: builds CmdPackets per the protocol contract
|
||||
// and sends them over the simulated radio. Toggles and
|
||||
// interlocks mirror remote/RemoteState.cpp and
|
||||
// remote/PacketBuilder.cpp - keep them in sync if those change.
|
||||
// RealRemote - the real handheld remote, over the air: the USB nRF24 dongle
|
||||
// (2026-current/dongle/, bridge mode) receives its packets, they
|
||||
// go into the firmware unchanged, and the robot's Telemetry goes
|
||||
// back as the radio's ack payload. The remote's display then
|
||||
// shows the simulated robot. Nothing about the remote is
|
||||
// mirrored here: its own firmware does all of it.
|
||||
// Toggles, the speed slider and the handset tilt sliders live in SimTestPanel.
|
||||
public class SimRemote : MonoBehaviour
|
||||
{
|
||||
public enum InputSource { Joystick, Keyboard, RobotSerialHarness }
|
||||
|
||||
[Serializable]
|
||||
public class AxisBinding
|
||||
{
|
||||
public string axis;
|
||||
public bool invert;
|
||||
public AxisBinding(string axis, bool invert) { this.axis = axis; this.invert = invert; }
|
||||
}
|
||||
public enum InputSource { Keyboard, RealRemote }
|
||||
|
||||
public InputSource source = InputSource.Keyboard;
|
||||
|
||||
// Defaults follow the old HandleHoystick: axes 4/6/5 were the walk stick
|
||||
// (b2/b1/b3), 2/1/3 the rotation stick (a1/a2/a3), with 1 and 2 inverted.
|
||||
// Which of 4/6 is forward vs sideways wasn't recoverable from the old code:
|
||||
// check the raw axis readout in the test panel and fix here.
|
||||
[Header("Joystick axes (Input Manager names)")]
|
||||
[Tooltip("Stick convention after invert: X + = right, Y + = forward, twist + = counterclockwise.")]
|
||||
public AxisBinding leftX = new AxisBinding("4", false);
|
||||
public AxisBinding leftY = new AxisBinding("6", false);
|
||||
public AxisBinding leftTwist = new AxisBinding("5", false);
|
||||
public AxisBinding rightX = new AxisBinding("2", true);
|
||||
public AxisBinding rightY = new AxisBinding("1", true);
|
||||
public AxisBinding rightTwist = new AxisBinding("3", false);
|
||||
[Range(0, 0.5f)] public float deadzone = 0.08f;
|
||||
[Header("Real remote (USB nRF24 dongle)")]
|
||||
[Tooltip("\"auto\" finds the dongle's CH340 port; or e.g. COM7.")]
|
||||
public string donglePort = "auto";
|
||||
|
||||
[Header("Operator state (same toggles as the Nextion screen)")]
|
||||
public bool armed;
|
||||
@@ -66,49 +47,61 @@ namespace HexapodSim
|
||||
public float armCenterTolerance = 0.05f; // ARM_CENTER_TOLERANCE / 1000
|
||||
public float keyStickRatePerSec = 4f; // how fast a held key moves the virtual stick
|
||||
|
||||
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST].
|
||||
// Latest processed sticks, -1..1: [L_X, L_Y, L_TWIST, R_X, R_Y, R_TWIST] (keyboard).
|
||||
public readonly float[] sticks = new float[6];
|
||||
public readonly float[] rawAxes = new float[6]; // Input axes "1".."6" as read
|
||||
public CmdPacket LastPacket { get; private set; }
|
||||
public string Message { get; private set; } = "";
|
||||
public bool JoystickAxesMissing { get; private set; }
|
||||
|
||||
// Real remote, for the panel.
|
||||
public DongleLink Dongle { get; private set; }
|
||||
public int RemotePacketsPerSec { get; private set; }
|
||||
public int LastPacketLength { get; private set; }
|
||||
public float SecondsSinceRemotePacket => Time.time - lastRemotePacket_;
|
||||
|
||||
float speedCurrent_ = 128;
|
||||
float lastWalkInputTime_ = -999f;
|
||||
byte seq_;
|
||||
string serialKeys_ = "";
|
||||
float messageUntil_;
|
||||
float lastRemotePacket_ = -999f;
|
||||
int packetsThisSecond_;
|
||||
float secondStart_;
|
||||
readonly byte[] ack_ = new byte[32];
|
||||
|
||||
public void ResetState()
|
||||
{
|
||||
armed = stand = false;
|
||||
speedCurrent_ = speedTarget;
|
||||
lastWalkInputTime_ = -999f;
|
||||
serialKeys_ = "";
|
||||
}
|
||||
|
||||
void Awake()
|
||||
{
|
||||
if (Input.GetJoystickNames().Length > 0 && !string.IsNullOrEmpty(Input.GetJoystickNames()[0]))
|
||||
source = InputSource.Joystick;
|
||||
}
|
||||
|
||||
void Update()
|
||||
{
|
||||
for (int i = 0; i < 6; i++) rawAxes[i] = ReadAxis((i + 1).ToString());
|
||||
// Open the dongle while the real remote is selected, release it otherwise.
|
||||
if (source == InputSource.RealRemote && Dongle == null) ConnectDongle();
|
||||
else if (source != InputSource.RealRemote && Dongle != null) DisconnectDongle();
|
||||
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
if (source == InputSource.Keyboard)
|
||||
{
|
||||
serialKeys_ += Input.inputString; // raw keystrokes for the firmware's harness
|
||||
return;
|
||||
HandleToggleKeys();
|
||||
ReadKeyboardSticks(Time.deltaTime);
|
||||
}
|
||||
|
||||
HandleToggleKeys();
|
||||
if (source == InputSource.Joystick) ReadJoystick();
|
||||
else ReadKeyboardSticks(Time.deltaTime);
|
||||
if (Time.time > messageUntil_) Message = "";
|
||||
}
|
||||
|
||||
void OnDisable() => DisconnectDongle();
|
||||
|
||||
public void ConnectDongle()
|
||||
{
|
||||
DisconnectDongle();
|
||||
Dongle = DongleLink.Open(donglePort);
|
||||
}
|
||||
|
||||
public void DisconnectDongle()
|
||||
{
|
||||
Dongle?.Dispose();
|
||||
Dongle = null;
|
||||
}
|
||||
|
||||
// ---- Operator commands (also called by SimTestPanel buttons) ----
|
||||
|
||||
public void ToggleArm()
|
||||
@@ -144,32 +137,6 @@ namespace HexapodSim
|
||||
if (Input.GetKeyDown(KeyCode.PageDown)) height = Mathf.Max(-1000, height - 100);
|
||||
}
|
||||
|
||||
// ---- Sticks ----
|
||||
|
||||
float ReadAxis(string name)
|
||||
{
|
||||
try { return Input.GetAxis(name); }
|
||||
catch (ArgumentException) { JoystickAxesMissing = true; return 0; } // axis not defined in Input Manager
|
||||
}
|
||||
|
||||
float Read(AxisBinding b)
|
||||
{
|
||||
float v = ReadAxis(b.axis) * (b.invert ? -1 : 1);
|
||||
float mag = Mathf.Abs(v);
|
||||
if (mag <= deadzone) return 0;
|
||||
return Mathf.Sign(v) * Mathf.Clamp01((mag - deadzone) / (1 - deadzone));
|
||||
}
|
||||
|
||||
void ReadJoystick()
|
||||
{
|
||||
sticks[0] = Read(leftX);
|
||||
sticks[1] = Read(leftY);
|
||||
sticks[2] = Read(leftTwist);
|
||||
sticks[3] = Read(rightX);
|
||||
sticks[4] = Read(rightY);
|
||||
sticks[5] = Read(rightTwist);
|
||||
}
|
||||
|
||||
// WASD + Q/E = left stick (walk, turn), IJKL + U/O = right stick.
|
||||
void ReadKeyboardSticks(float dt)
|
||||
{
|
||||
@@ -187,17 +154,36 @@ namespace HexapodSim
|
||||
// Called by HexapodSimulation once per remote TX period.
|
||||
public void Transmit(HexapodSimNative native, float dt)
|
||||
{
|
||||
if (source == InputSource.RobotSerialHarness)
|
||||
{
|
||||
native.SerialInput(serialKeys_); // no radio: the firmware's harness injects its own packets
|
||||
serialKeys_ = "";
|
||||
return;
|
||||
}
|
||||
if (source == InputSource.RealRemote) { ForwardRealRemote(native); return; }
|
||||
speedCurrent_ = Mathf.MoveTowards(speedCurrent_, speedTarget, speedRatePerSec * dt);
|
||||
LastPacket = BuildPacket();
|
||||
native.RadioSend(LastPacket);
|
||||
}
|
||||
|
||||
// Everything the dongle heard since last time goes into the firmware in
|
||||
// order, and each packet's ack carries the telemetry the robot loaded in
|
||||
// reply - the same exchange as on the real robot (robot/Comms.cpp).
|
||||
void ForwardRealRemote(HexapodSimNative native)
|
||||
{
|
||||
if (Time.time - secondStart_ >= 1f)
|
||||
{
|
||||
RemotePacketsPerSec = packetsThisSecond_;
|
||||
packetsThisSecond_ = 0;
|
||||
secondStart_ = Time.time;
|
||||
}
|
||||
if (Dongle == null || !Dongle.IsOpen) return;
|
||||
while (Dongle.TryTakePacket(out byte[] pkt))
|
||||
{
|
||||
native.RadioSend(pkt);
|
||||
int n = native.GetAckPayload(ack_);
|
||||
if (n > 0) Dongle.SendAckPayload(ack_, n);
|
||||
LastPacketLength = pkt.Length;
|
||||
if (HexapodSimNative.TryDecode(pkt, out var p)) LastPacket = p;
|
||||
lastRemotePacket_ = Time.time;
|
||||
packetsThisSecond_++;
|
||||
}
|
||||
}
|
||||
|
||||
static short Wire(float v) => (short)Mathf.RoundToInt(Mathf.Clamp(v, -1f, 1f) * 1000f);
|
||||
|
||||
// Same mapping as remote/PacketBuilder.cpp.
|
||||
@@ -208,6 +194,7 @@ namespace HexapodSim
|
||||
seq = seq_++,
|
||||
speed_scale = (byte)Mathf.RoundToInt(speedCurrent_),
|
||||
body_height = (short)Mathf.Clamp(height, -1000, 1000), // a setting: sent even while disarmed
|
||||
cal_servo = 0xFF, // CAL_NO_SERVO: never calibrates
|
||||
};
|
||||
byte f = 0;
|
||||
if (armed) f |= CmdPacket.FLAG_ARMED;
|
||||
|
||||
@@ -16,7 +16,7 @@ namespace HexapodSim
|
||||
Vector2 scroll_;
|
||||
GUIStyle mono_;
|
||||
|
||||
static readonly string[] SourceNames = { "Joystick", "Keyboard", "Robot serial" };
|
||||
static readonly string[] SourceNames = { "Keyboard", "Real remote (radio)" };
|
||||
|
||||
void Awake()
|
||||
{
|
||||
@@ -35,8 +35,10 @@ namespace HexapodSim
|
||||
if (mono_ == null)
|
||||
mono_ = new GUIStyle(GUI.skin.label) { font = Font.CreateDynamicFontFromOSFont("Consolas", 12), fontSize = 12, wordWrap = false };
|
||||
|
||||
GUILayout.BeginArea(new Rect(10, 10, 390, Screen.height - 20), GUI.skin.box);
|
||||
scroll_ = GUILayout.BeginScrollView(scroll_);
|
||||
GUILayout.BeginArea(new Rect(10, 10, 420, Screen.height - 20), GUI.skin.box);
|
||||
// Vertical scrolling only: the monospace lines run a little past the
|
||||
// width, and a sideways bar for that is just noise.
|
||||
scroll_ = GUILayout.BeginScrollView(scroll_, false, false, GUIStyle.none, GUI.skin.verticalScrollbar);
|
||||
|
||||
GUILayout.Label("<b>Hexapod firmware simulation</b> (F1 hides)", new GUIStyle(GUI.skin.label) { richText = true });
|
||||
if (sim_.LoadError != null)
|
||||
@@ -65,18 +67,13 @@ namespace HexapodSim
|
||||
// ---- Input source ----
|
||||
GUILayout.Space(6);
|
||||
remote_.source = (SimRemote.InputSource)GUILayout.Toolbar((int)remote_.source, SourceNames);
|
||||
if (remote_.source == SimRemote.InputSource.RobotSerialHarness)
|
||||
if (remote_.source == SimRemote.InputSource.RealRemote)
|
||||
{
|
||||
GUILayout.Label("Keys go to robot.ino's TEST_SERIAL_INPUT harness (no radio):\n" +
|
||||
"a arm s sit/stand w/x fwd/back q/e strafe L/R\n" +
|
||||
"z/c yaw g gait m mode space stop", mono_);
|
||||
DongleSection();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (remote_.source == SimRemote.InputSource.Keyboard)
|
||||
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
|
||||
if (remote_.source == SimRemote.InputSource.Joystick && remote_.JoystickAxesMissing)
|
||||
GUILayout.Label("Input axes \"1\"..\"6\" are missing from the Input Manager.");
|
||||
GUILayout.Label("WASD walk Q/E turn IJKL + U/O right stick\n1 arm 2 stand 3 gait 4 mode 5 R-stick 6/7 gyros\nBackspace disarm +/- speed PgUp/PgDn height", mono_);
|
||||
|
||||
// ---- Toggles: same set as the remote's touchscreen ----
|
||||
GUILayout.BeginHorizontal();
|
||||
@@ -116,11 +113,10 @@ namespace HexapodSim
|
||||
// ---- Live values ----
|
||||
GUILayout.Space(6);
|
||||
var st = remote_.sticks;
|
||||
var raw = remote_.rawAxes;
|
||||
var p = remote_.LastPacket;
|
||||
GUILayout.Label(
|
||||
$"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" +
|
||||
$"axes 1-6 {raw[0],5:F2} {raw[1],5:F2} {raw[2],5:F2} {raw[3],5:F2} {raw[4],5:F2} {raw[5],5:F2}\n" +
|
||||
(remote_.source == SimRemote.InputSource.Keyboard
|
||||
? $"sticks L {st[0],5:F2} {st[1],5:F2} {st[2],5:F2} R {st[3],5:F2} {st[4],5:F2} {st[5],5:F2}\n" : "") +
|
||||
$"packet #{p.seq,3} vx {p.vx,5} vy {p.vy,5} yaw {p.yaw_rate,5}\n" +
|
||||
$" r {p.rx,5} {p.ry,5} {p.rz,5} tilt {p.remote_roll,5} {p.remote_pitch,5}\n" +
|
||||
$" height {p.body_height,5} speed {p.speed_scale,3} flags {System.Convert.ToString(p.flags, 2).PadLeft(8, '0')}",
|
||||
@@ -136,6 +132,36 @@ namespace HexapodSim
|
||||
GUILayout.EndArea();
|
||||
}
|
||||
|
||||
// The real remote drives the simulation over the air through the USB dongle;
|
||||
// every control is on the remote itself, so this only shows the link.
|
||||
void DongleSection()
|
||||
{
|
||||
var d = remote_.Dongle;
|
||||
var rich = new GUIStyle(GUI.skin.label) { richText = true, wordWrap = true };
|
||||
GUILayout.BeginHorizontal();
|
||||
GUILayout.Label("dongle port", GUILayout.Width(80));
|
||||
remote_.donglePort = GUILayout.TextField(remote_.donglePort, GUILayout.Width(70));
|
||||
if (GUILayout.Button("Reconnect")) remote_.ConnectDongle();
|
||||
GUILayout.EndHorizontal();
|
||||
|
||||
if (d == null) { GUILayout.Label("connecting..."); return; }
|
||||
if (!d.IsOpen || d.Error != null)
|
||||
{
|
||||
GUILayout.Label("<color=#ff6060>" + (d.Error ?? "not open") + "</color>", rich);
|
||||
if (!d.IsOpen) return;
|
||||
}
|
||||
float age = remote_.SecondsSinceRemotePacket;
|
||||
string link = age < 0.5f ? "<color=#60ff60>receiving</color>" : "<color=#ffb040>no packets - is the remote on?</color>";
|
||||
GUILayout.Label(
|
||||
$"{d.PortName}: {link}\n" +
|
||||
$"remote {remote_.RemotePacketsPerSec} pkt/s here, dongle {d.DonglePacketsPerSec} pkt/s, acks {d.DongleAcksPerSec}/s" +
|
||||
(remote_.LastPacketLength != 0 && remote_.LastPacketLength != System.Runtime.InteropServices.Marshal.SizeOf<CmdPacket>()
|
||||
? $"\n<color=#ff6060>packets are {remote_.LastPacketLength} bytes: remote and sim built from different protocol versions</color>" : "") +
|
||||
(d.BadFrames > 0 ? $"\n{d.BadFrames} corrupt frames" : ""),
|
||||
rich);
|
||||
GUILayout.Label("Use the remote's sticks and touchscreen. Its display shows this simulated robot.", rich);
|
||||
}
|
||||
|
||||
static string StanceString(int mask)
|
||||
{
|
||||
var sb = new StringBuilder(6);
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5f676c461da1c254990d7719d77e6250
|
||||
folderAsset: yes
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,72 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using UnityEngine;
|
||||
|
||||
namespace Assets.JoyStick
|
||||
{
|
||||
class HandleHoystick : MonoBehaviour
|
||||
{
|
||||
public RotationControl bot;
|
||||
public Walk walk;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
float a2 = Remap(Input.GetAxis("1"), 1, -127, -1, 127);
|
||||
float a1 = Remap(Input.GetAxis("2"), 1, -127, -1, 127);
|
||||
float a3 = Remap(Input.GetAxis("3"), -1, -127, 1, 127);
|
||||
|
||||
float b2 = Remap(Input.GetAxis("4"), -1, -127, 1, 127);
|
||||
float b3 = Remap(Input.GetAxis("5"), -1, -127, 1, 127);
|
||||
float b1 = Remap(Input.GetAxis("6"), -1, -127, 1, 127);
|
||||
|
||||
//Debug.Log(Input.GetAxis("4"));
|
||||
//Debug.Log(Remap(Input.GetAxis("4"), -1, -127, 1, 127));
|
||||
|
||||
bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
1);
|
||||
|
||||
float speed = (float)(Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
float rotation = -1 * limit(b3 / 4, -5, 5);
|
||||
walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
walk.walkDirection = (-1 * ConvertRadiansToDegrees((float)Math.Atan2(b2, b1)));
|
||||
|
||||
float rotationabs = Math.Abs(walk.centerRotation);
|
||||
float speed2 = ((6 / 3) * walk.walkspeed) + ((6 / 5) * rotationabs);
|
||||
speed2 = speed2 < 6 ? speed2 : 6;
|
||||
walk.speed2 = (int)(10 - speed2);
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
input = input / 6;
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float)((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
public float Remap(float from, float fromMin, float toMin, float fromMax, float toMax)
|
||||
{
|
||||
var fromAbs = from - fromMin;
|
||||
var fromMaxAbs = fromMax - fromMin;
|
||||
|
||||
var normal = fromAbs / fromMaxAbs;
|
||||
|
||||
var toMaxAbs = toMax - toMin;
|
||||
var toAbs = toMaxAbs * normal;
|
||||
|
||||
var to = toAbs + toMin;
|
||||
|
||||
return to;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c5ce8a2605d301e44a4a210aa345ed62
|
||||
timeCreated: 1641405303
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -79,11 +79,8 @@ public class Leg : MonoBehaviour {
|
||||
// (int)Remap((float)a1, -90, 90, 2500, 500),
|
||||
// (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
|
||||
if(false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, -90, 90, 0, 180),
|
||||
(int)Remap((float)a2, 90, -90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, -90, 90, 0, 180)); // 0 is middel
|
||||
// (servo angles used to go out over USB serial here; dropped 2026-11-17,
|
||||
// the 2026 robot runs its own firmware and takes commands over the radio)
|
||||
}
|
||||
else if (invertForLeftLeg && startServo != -1)
|
||||
{
|
||||
@@ -95,11 +92,6 @@ public class Leg : MonoBehaviour {
|
||||
// (int)Remap((float)a1, -90, 90, 500, 2500),
|
||||
// (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
|
||||
// (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
|
||||
if (false)
|
||||
Serial.Instance.send(startServo,
|
||||
(int)Remap((float)a1, 90, -90, 0, 180),
|
||||
(int)Remap((float)a2, -90, 90, 0, 180), // 0 is middel
|
||||
(int)Remap((float)a3, 90, -90, 0, 180)); // 0 is middel
|
||||
}
|
||||
|
||||
Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
|
||||
|
||||
@@ -12875,7 +12875,6 @@ GameObject:
|
||||
serializedVersion: 6
|
||||
m_Component:
|
||||
- component: {fileID: 73925799}
|
||||
- component: {fileID: 73925800}
|
||||
m_Layer: 0
|
||||
m_Name: SerialIn
|
||||
m_TagString: Untagged
|
||||
@@ -12898,28 +12897,6 @@ Transform:
|
||||
m_Children: []
|
||||
m_Father: {fileID: 0}
|
||||
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
|
||||
--- !u!114 &73925800
|
||||
MonoBehaviour:
|
||||
m_ObjectHideFlags: 0
|
||||
m_CorrespondingSourceObject: {fileID: 0}
|
||||
m_PrefabInstance: {fileID: 0}
|
||||
m_PrefabAsset: {fileID: 0}
|
||||
m_GameObject: {fileID: 73925798}
|
||||
m_Enabled: 0
|
||||
m_EditorHideFlags: 0
|
||||
m_Script: {fileID: 11500000, guid: 7fa025fa8dcb3a249b67fc0e2fe67b44, type: 3}
|
||||
m_Name:
|
||||
m_EditorClassIdentifier:
|
||||
bot: {fileID: 311513962}
|
||||
walk: {fileID: 311513965}
|
||||
a1: 0
|
||||
a2: 0
|
||||
a3: 0
|
||||
b1: 0
|
||||
b2: 0
|
||||
b3: 0
|
||||
enabled: 0
|
||||
enabled_send: 0
|
||||
--- !u!1 &74230589
|
||||
GameObject:
|
||||
m_ObjectHideFlags: 0
|
||||
@@ -325049,7 +325026,6 @@ GameObject:
|
||||
serializedVersion: 6
|
||||
m_Component:
|
||||
- component: {fileID: 1645933599}
|
||||
- component: {fileID: 1645933598}
|
||||
m_Layer: 0
|
||||
m_Name: Input
|
||||
m_TagString: Untagged
|
||||
@@ -325057,20 +325033,6 @@ GameObject:
|
||||
m_NavMeshLayer: 0
|
||||
m_StaticEditorFlags: 0
|
||||
m_IsActive: 1
|
||||
--- !u!114 &1645933598
|
||||
MonoBehaviour:
|
||||
m_ObjectHideFlags: 0
|
||||
m_CorrespondingSourceObject: {fileID: 0}
|
||||
m_PrefabInstance: {fileID: 0}
|
||||
m_PrefabAsset: {fileID: 0}
|
||||
m_GameObject: {fileID: 1645933597}
|
||||
m_Enabled: 1
|
||||
m_EditorHideFlags: 0
|
||||
m_Script: {fileID: 11500000, guid: c5ce8a2605d301e44a4a210aa345ed62, type: 3}
|
||||
m_Name:
|
||||
m_EditorClassIdentifier:
|
||||
bot: {fileID: 311513962}
|
||||
walk: {fileID: 311513965}
|
||||
--- !u!4 &1645933599
|
||||
Transform:
|
||||
m_ObjectHideFlags: 0
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 942facdab943ceb4ca29453f4ee4d1b3
|
||||
folderAsset: yes
|
||||
timeCreated: 1641404792
|
||||
licenseType: Free
|
||||
DefaultImporter:
|
||||
externalObjects: {}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,119 +0,0 @@
|
||||
using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using Random = System.Random;
|
||||
|
||||
public class Serial
|
||||
{
|
||||
private static Serial instance;
|
||||
private int rnd = 0;
|
||||
|
||||
private Serial()
|
||||
{
|
||||
}
|
||||
|
||||
~Serial()
|
||||
{
|
||||
serialPort.Close();
|
||||
serialPort.Dispose();
|
||||
}
|
||||
|
||||
public static Serial Instance
|
||||
{
|
||||
get
|
||||
{
|
||||
if (instance == null)
|
||||
{
|
||||
instance = new Serial();
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
public SerialPort serialPort;
|
||||
|
||||
public int[] values = new int[18];
|
||||
|
||||
public static string ByteArrayToString(byte[] ba)
|
||||
{
|
||||
return BitConverter.ToString(ba).Replace("-", " ");
|
||||
}
|
||||
|
||||
public void send(int start, int a, int b, int c)
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM4", 250000);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.Parity = Parity.Even;
|
||||
serialPort.StopBits = StopBits.One;
|
||||
serialPort.DataBits = 8;
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
System.Threading.Thread.Sleep(1000);
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
values[start] = a;
|
||||
values[++start] = b;
|
||||
values[++start] = c;
|
||||
|
||||
if (serialPort is SerialPort && start == 17)
|
||||
{
|
||||
// string send = "s";
|
||||
byte[] send = new byte[18+1+2];
|
||||
send[18] = 0x00;
|
||||
send[19] = 0xfe;
|
||||
send[20] = 0xff;
|
||||
// send[21] = 0xff;
|
||||
|
||||
// if (rnd == 0)
|
||||
// {
|
||||
//rnd = 1;
|
||||
for (int i = 0; i < 18; i++)
|
||||
{
|
||||
//string crc = ((i) * (values[i])) + "";
|
||||
//send = send + (i).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
byte[] intBytes = BitConverter.GetBytes(values[i]);
|
||||
if (BitConverter.IsLittleEndian)
|
||||
Array.Reverse(intBytes);
|
||||
send[i] = intBytes[3];
|
||||
send[18] += send[i];
|
||||
}
|
||||
|
||||
// Debug.Log(ByteArrayToString(send));
|
||||
serialPort.Write(send, 0, 18+1+2);
|
||||
serialPort.BaseStream.Flush();
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// rnd = 0;
|
||||
// for (int i = 10; i < 18; i++)
|
||||
// {
|
||||
// string crc = ((i + 3) * (values[i])) + "";
|
||||
//
|
||||
// send = send + (i + 2).ToString() + "_" + (values[i]).ToString() + "," + crc + "=";
|
||||
// }
|
||||
//
|
||||
// Debug.Log(send);
|
||||
// serialPort.Write(send);
|
||||
// }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: c99b6d9374ff6d3419c7692fdf3c1791
|
||||
timeCreated: 1542057315
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,70 +0,0 @@
|
||||
/*using System;
|
||||
using UnityEngine;
|
||||
using System.Collections;
|
||||
using System.IO.Ports;
|
||||
using System.Linq;
|
||||
|
||||
public class Serial2 : MonoBehaviour{
|
||||
|
||||
public float x;
|
||||
public float y;
|
||||
|
||||
public RotationControl bot;
|
||||
|
||||
~Serial2() {
|
||||
serialPort.Close();
|
||||
}
|
||||
|
||||
public bool enabled = false;
|
||||
|
||||
SerialPort serialPort;
|
||||
|
||||
public void Update()
|
||||
{
|
||||
if (serialPort == null)
|
||||
{
|
||||
serialPort = new SerialPort(@"\\.\" + "COM7", 115200);
|
||||
|
||||
try
|
||||
{
|
||||
serialPort.NewLine = "\n";
|
||||
|
||||
serialPort.Open();
|
||||
serialPort.DiscardOutBuffer();
|
||||
serialPort.DiscardInBuffer();
|
||||
|
||||
enabled = true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
enabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (enabled)
|
||||
{
|
||||
if (serialPort is SerialPort)
|
||||
{
|
||||
string received = serialPort.ReadLine();
|
||||
string[] data = received.Split(',');
|
||||
x = ConvertRadiansToDegrees(float.Parse(data.First()));
|
||||
y = ConvertRadiansToDegrees(float.Parse(data.Last()));
|
||||
|
||||
bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public float limit(float input, float min, float max)
|
||||
{
|
||||
return (input < max & input > min) ? input : input < min ? min : max;
|
||||
}
|
||||
|
||||
public float ConvertRadiansToDegrees(float radians)
|
||||
{
|
||||
float degrees = (float) ((180 / Math.PI) * radians);
|
||||
return (degrees);
|
||||
}
|
||||
|
||||
}
|
||||
*/
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ac6468bb4d2734947a923ea42d6d8192
|
||||
timeCreated: 1557262171
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,170 +0,0 @@
|
||||
//using System;
|
||||
//using UnityEngine;
|
||||
//using System.Collections;
|
||||
//using System.IO.Ports;
|
||||
//using System.Linq;
|
||||
//using UnityEngine.UI;
|
||||
|
||||
//public class Serial3 : MonoBehaviour{
|
||||
|
||||
// public RotationControl bot;
|
||||
// public Walk walk;
|
||||
|
||||
// public float a1, a2, a3, b1, b2, b3;
|
||||
|
||||
// // ~Serial3()
|
||||
// // {
|
||||
// // serialPort.Close();
|
||||
// // serialPort.Dispose();
|
||||
// // }
|
||||
|
||||
// public bool enabled = false;
|
||||
// public bool enabled_send = false;
|
||||
|
||||
// SerialPort serialPort;
|
||||
|
||||
// public void Update()
|
||||
// {
|
||||
// if (serialPort == null)
|
||||
// {
|
||||
// serialPort = new SerialPort(@"\\.\" + "COM7", 230400);
|
||||
|
||||
// try
|
||||
// {
|
||||
// serialPort.NewLine = "\r\n";
|
||||
|
||||
//// serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
|
||||
|
||||
// serialPort.Open();
|
||||
// serialPort.DiscardOutBuffer();
|
||||
// serialPort.DiscardInBuffer();
|
||||
|
||||
// enabled = true;
|
||||
// }
|
||||
// catch
|
||||
// {
|
||||
// enabled = false;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// serialPort = Serial.Instance.serialPort;
|
||||
// Serial.Instance.enabled = enabled_send;
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value - 127;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value - 127;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value - 127;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value - 127;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value - 127;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value - 127;
|
||||
// break;
|
||||
// }
|
||||
|
||||
// bot.rotation = new Quaternion(limit(a2, -20, 20), -1 * limit(a3, -20, 20), -1 * limit(a1, -20, 20),
|
||||
// 1);
|
||||
|
||||
// float speed = (float) (Math.Sqrt(b1 * b1 + b2 * b2) / 127) * 3;
|
||||
// walk.walkspeed = speed < 0.5f ? 0 : speed;
|
||||
|
||||
// float rotation = -1 * limit(b3/4, -5, 5);
|
||||
// walk.centerRotation = rotation < 0.5f && rotation > -0.5f ? 0 : rotation;
|
||||
|
||||
// walk.walkDirection = (-1 * ConvertRadiansToDegrees((float) Math.Atan2(b2, b1)));
|
||||
|
||||
// float rotationabs = Math.Abs(walk.centerRotation);
|
||||
// float speed2 = ((6 / 3) * walk.walkspeed) + ((6/5) * rotationabs);
|
||||
// speed2 = speed2 < 6 ? speed2 : 6;
|
||||
// walk.speed2 = (int)(10 - speed2);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// private void DataReceivedHandler(object sender, SerialDataReceivedEventArgs e)
|
||||
// {
|
||||
// SerialPort serialPort = (SerialPort)sender;
|
||||
|
||||
// if (enabled)
|
||||
// {
|
||||
// if (serialPort is SerialPort)
|
||||
// {
|
||||
// string received = serialPort.ReadLine();
|
||||
// string[] data = received.Split(',');
|
||||
|
||||
// foreach (string d in data)
|
||||
// {
|
||||
// // if (data == "\r")
|
||||
// // continue;
|
||||
|
||||
// string[] ch = d.Split('_');
|
||||
|
||||
// int channel = int.Parse(ch.First());
|
||||
// float value = float.Parse(ch.Last());
|
||||
|
||||
// switch (channel)
|
||||
// {
|
||||
// case 0:
|
||||
// a1 = value;
|
||||
// break;
|
||||
// case 1:
|
||||
// a2 = value;
|
||||
// break;
|
||||
// case 2:
|
||||
// a3 = value;
|
||||
// break;
|
||||
// case 3:
|
||||
// b1 = value;
|
||||
// break;
|
||||
// case 4:
|
||||
// b2 = value;
|
||||
// break;
|
||||
// case 5:
|
||||
// b3 = value;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// // bot.rotation = new Quaternion(-1* limit(y, -30, 30), 0, -1* limit(x, -30, 30), 1);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
// public float limit(float input, float min, float max)
|
||||
// {
|
||||
// input = input / 6;
|
||||
// return (input < max & input > min) ? input : input < min ? min : max;
|
||||
// }
|
||||
|
||||
// public float ConvertRadiansToDegrees(float radians)
|
||||
// {
|
||||
// float degrees = (float)((180 / Math.PI) * radians);
|
||||
// return (degrees);
|
||||
// }
|
||||
|
||||
//}
|
||||
@@ -1,13 +0,0 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 7fa025fa8dcb3a249b67fc0e2fe67b44
|
||||
timeCreated: 1558207463
|
||||
licenseType: Free
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -1,3 +1,3 @@
|
||||
<projectSettings>
|
||||
<projectSetting name="Google.VersionHandler.VerboseLoggingEnabled" value="False" />
|
||||
<projectSettings>
|
||||
<projectSetting name="Google.VersionHandler.VerboseLoggingEnabled" value="False" />
|
||||
</projectSettings>
|
||||
@@ -1 +1,5 @@
|
||||
Hexapod
|
||||
|
||||
The Unity side of the firmware simulation: it runs the unchanged robot firmware
|
||||
(`../sim/build/hexapod_sim.dll`) on the 3D model, driven by the keyboard or by the
|
||||
real remote over the radio (USB nRF24 dongle). See `../sim/README.md`.
|
||||
+80
-9
@@ -1,20 +1,31 @@
|
||||
# Hexapod
|
||||
|
||||
A six-legged walking robot (Teensy 3.6) and its handheld remote
|
||||
(Teensy 4.1 + Nextion touchscreen), linked over nRF24L01.
|
||||
(Teensy 4.0 + Nextion touchscreen), linked over nRF24L01: commands from the
|
||||
remote, telemetry back from the robot. A USB nRF24 stick lets the PC (the
|
||||
Unity simulation) take the robot's place on the radio.
|
||||
|
||||

|
||||

|
||||

|
||||
|
||||
The remote above is the new design (`hardware/openscad/remote_v2.scad`, not
|
||||
printed yet). The original printed remote is on the
|
||||
[archive page](hardware/openscad/ARCHIVE.md).
|
||||
|
||||
```
|
||||
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
|
||||
robot/ robot firmware: IK, gait, body pose, IMU stabilization, state machine,
|
||||
servo centre calibration, telemetry
|
||||
remote/ remote firmware: 2 x 3-axis sticks, MPU6050 tilt, Nextion UI, RF24 link
|
||||
(+ remote.HMI, the touchscreen project)
|
||||
shared/ HexapodProtocol: CmdPacket + Telemetry wire formats + radio settings
|
||||
sim/ the unchanged robot firmware compiled into a DLL for the simulation
|
||||
Hexapod_Simulation_Unity/ Unity project that runs that DLL on the 3D model
|
||||
hardware/openscad/ OpenSCAD model of the robot and remote, rebuilt from the Fusion designs
|
||||
dongle/ USB nRF24 stick firmware (STM32F030): the PC-side radio, see dongle/README.md
|
||||
hardware/openscad/ OpenSCAD model of the robot (rebuilt from the Fusion designs) and the new remote
|
||||
hardware/openscad/stl/ print-ready STL exports
|
||||
hardware/fusion360/ Fusion 360 designs: body, legs, servo, servo horn, remote case
|
||||
scripts/ PlatformIO helper (per-environment sketch folder)
|
||||
scripts/ PlatformIO helper (per-environment sketch folder), make_remote_hmi.py
|
||||
tools/nextion-hmi-tools/ git submodule: .HMI reader/writer/renderer (nxhmi), used by the HMI generator
|
||||
```
|
||||
|
||||
This is the `2026-current/` folder of the hexapod-robot repo; the earlier
|
||||
@@ -41,6 +52,62 @@ From this `2026-current` folder in PowerShell (the dev PC is Windows):
|
||||
Then open `Hexapod_Simulation_Unity` and press Play. See
|
||||
[sim/README.md](sim/README.md).
|
||||
|
||||
## Printing the remote (v2)
|
||||
|
||||
STLs in [hardware/openscad/stl/remote_v2/](hardware/openscad/stl/remote_v2/),
|
||||
already in print orientation; everything prints in ASA without supports.
|
||||
Regenerate them from `remote_v2.scad` with `-D 'PART="..."'`.
|
||||
|
||||
| File | Part | Notes |
|
||||
|------|------|-------|
|
||||
| `remote_v2_top_inlays.3mf` | top shell with both inlays | One object with three parts on filaments 1/2/3: case black, honeycomb red, ON/OFF white. The inlays are the first 3 layers (0.6 mm) of the face-down shell, so the colour swaps only happen there. Made by `hardware/openscad/export_3mf.py` from the three STLs below |
|
||||
| `remote_v2_top.stl` + `_inlay.stl` + `_text.stl` | the same, as STLs | Load the three at once and keep them as one multi-part object |
|
||||
| `remote_v2_top_multicolor.3mf` | OrcaSlicer project (owner) | all parts on two plates; after re-exporting STLs, update its parts with Orca's "Reload from disk" |
|
||||
| `remote_v2_bottom.stl` | bottom shell | flat underside on the bed |
|
||||
| `remote_v2_kickstand.stl` | table stand | |
|
||||
| `remote_v2_eyelet.stl` | neck-strap eyelet | on the top face (slots to balance it) or on the front edge |
|
||||
| `remote_v2_bail.stl` | body brace | swings down and back to rest against your body |
|
||||
|
||||
Hardware (besides the electronics of the old remote):
|
||||
|
||||
- 14 x M3 heat-set inserts (4.0 mm hole, ~5.7 mm long): 4 shell screws,
|
||||
2 + 2 eyelet (top and front), 2 brace pivots, 4 main-board clamps
|
||||
- M3 x 12 x 4 (shells), M3 x 8 button head x 2 (eyelet), M3 x 16 button
|
||||
head + washer x 2 (brace), M3 x 8 button head + 7 mm washer x 4 (main
|
||||
board clamps, no holes in the board), M3 x 40 + nut (kickstand pin)
|
||||
- M2.5 x 14 x 8 (joysticks, into their own boxes), M3 x 5 machine screws x 4
|
||||
(display, tap the posts M3), M3 self-tapping x 2 (UPS board), M2 self-tapping x 2 (switch)
|
||||
- The display is mounted turned round so its connector clears the Teensy's USB
|
||||
plug. `remote.HMI` is generated for that (display direction 270 in the
|
||||
Editor's terms, see [remote/NEXTION.md](remote/NEXTION.md)), nothing to set.
|
||||
|
||||
Design notes, checks and open points: [hardware/openscad/PLANNING.md](hardware/openscad/PLANNING.md).
|
||||
|
||||
## Planned
|
||||
|
||||
- **PC radio link:** a USB nRF24L01 stick on the PC as the PC-side radio, on
|
||||
the same air settings as the robot and the remote. It's reverse-engineered
|
||||
and runs our own firmware (STM32F030 + Si24R1 + AT2401C, updated wire-free
|
||||
over USB), see **[dongle/README.md](dongle/README.md)**. Done (2026-11-17):
|
||||
its **bridge mode** lets the real remote drive the Unity simulation over the
|
||||
air, with the sim's telemetry going back to the remote's display as the ack
|
||||
payload ([sim/README.md](sim/README.md), "Using it in Unity"). There's no USB
|
||||
link to the remote or the robot any more; everything goes over the radio.
|
||||
Still to do:
|
||||
- Unity driving the **real** robot (the dongle transmitting instead of
|
||||
listening) and mirroring it;
|
||||
- three nodes at once: today the dongle answers the remote *instead of* the
|
||||
robot. Needs an address per node (or per pipe), a packet type byte, and a
|
||||
rule for who has priority. Owner's decisions: the remote wins, with a
|
||||
toggle on the remote that hands the robot to Unity and lets the remote's
|
||||
sticks control Unity instead; the PC is the hub while it's online, the
|
||||
robot is the hub when it isn't; goals: mirror the real robot in Unity,
|
||||
drive it from Unity, remote data to the PC, and PC data to the remote.
|
||||
- Remote battery level: see [remote/README.md](remote/README.md), "Planned".
|
||||
- Robot power: a stable 6 V supply for the 18 servos, see
|
||||
[hardware/openscad/PLANNING.md](hardware/openscad/PLANNING.md), "Notes for
|
||||
future designs".
|
||||
|
||||
## Where to go next
|
||||
|
||||
- **[robot/README.md](robot/README.md)**: placeholder geometry to measure,
|
||||
@@ -48,7 +115,11 @@ Then open `Hexapod_Simulation_Unity` and press Play. See
|
||||
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.
|
||||
- **[remote/NEXTION.md](remote/NEXTION.md)**: the touchscreen HMI (generated
|
||||
by `scripts/make_remote_hmi.py`).
|
||||
- **[sim/README.md](sim/README.md)**: the robot firmware in simulation, headless
|
||||
and in Unity, with the keyboard or the real remote over the radio.
|
||||
- **[dongle/README.md](dongle/README.md)**: the USB nRF24 stick, its firmware,
|
||||
flashing and the bridge protocol.
|
||||
- **[shared/HexapodProtocol/HexapodProtocol.h](shared/HexapodProtocol/HexapodProtocol.h)**:
|
||||
the protocol. Change it only by reflashing both boards.
|
||||
@@ -0,0 +1,2 @@
|
||||
# build artifacts (regenerate with firmware/build.ps1)
|
||||
build/
|
||||
@@ -0,0 +1,222 @@
|
||||
# USB nRF24 dongle
|
||||
|
||||
A cheap "USB NRF24L01+ 2.4G" stick (AliExpress, board marked **USB_24RF
|
||||
Ver 3.1**, Rainsun chip antenna) reflashed with our own firmware so it can be
|
||||
the PC-side radio for the hexapod: same nRF24 air settings as the robot and the
|
||||
remote, talking to Unity over USB serial.
|
||||
|
||||
It shipped with a closed "AT command" bridge firmware. We replaced that with the
|
||||
two-stage firmware in `firmware/`. **The factory firmware could not be backed
|
||||
up** (read-protected, see below), so this is a one-way change — done on the
|
||||
owner's say-so.
|
||||
|
||||
## Hardware (reverse-engineered from the board)
|
||||
|
||||
| Part | Chip | Notes |
|
||||
|------|------|-------|
|
||||
| MCU | **STM32F030F4P6** | Cortex-M0, 16 KB flash, 4 KB RAM, TSSOP-20 |
|
||||
| USB-serial | **CH330N** | shows up as "USB-SERIAL CH340" on Windows (COM port) |
|
||||
| Radio | **Si24R1** | nRF24L01+ clone, driven over SPI; 16 MHz crystal |
|
||||
| PA/LNA | **AT2401C** | 2.4 GHz power amp + low-noise amp (range booster) |
|
||||
| antenna | Rainsun chip antenna | |
|
||||
|
||||
### Pin map (MCU)
|
||||
|
||||
Found by probing the board over SWD and confirmed at runtime by our firmware:
|
||||
|
||||
| Function | STM32 pin |
|
||||
|----------|-----------|
|
||||
| USB serial (USART1) TX / RX | PA9 / PA10 → CH330N |
|
||||
| SPI1 SCK / MISO / MOSI | PA5 / PA6 / PA7 → Si24R1 |
|
||||
| nRF CSN / CE / IRQ | PA3 / PA4 / PA2 |
|
||||
| status LED (active-high) | **PA0** — confirmed by eye |
|
||||
| AT2401C TX/RX enable + unused | PA1, PB1, PF0, PF1 — **not distinguishable** (see below) |
|
||||
|
||||
PA0 drives the LED (HIGH = on). The AT2401C control lines would be among PA1,
|
||||
PB1, PF0, PF1 (the TSSOP-20 has no other GPIO). Listening for ambient 2.4 GHz
|
||||
with the radio's carrier-detect bit was too noisy to tell them apart. The real
|
||||
link test (2026-11-17: receiving the remote while switching all 16 on/off
|
||||
combinations, at the desk and a few rooms away with the remote at minimum
|
||||
power) got every packet in every combination, so either the link has plenty of
|
||||
margin or the amplifier is switched in hardware. The firmware leaves them low;
|
||||
the link works.
|
||||
|
||||
### App commands (USB serial, 115200, single keys, when not bridging)
|
||||
|
||||
| Key | Action |
|
||||
|-----|--------|
|
||||
| `v` | banner: version and a few radio registers |
|
||||
| `r` | dump nRF registers |
|
||||
| `x` | RX test on the hexapod's air settings; prints packets/s once a second |
|
||||
| `0`-`4` | toggle PA0 (LED) / PA1 / PB1 / PF0 / PF1 |
|
||||
| `a` | sweep every free pin high for 3 s each |
|
||||
| `f` / `n` | carrier-detect finder on / next channel |
|
||||
| `b` | reset into safeboot (what `safeboot_flash.py` uses) |
|
||||
|
||||
**SignalRGB** (if installed) opens every CH340 COM port it sees and holds it;
|
||||
quit it while using the dongle.
|
||||
|
||||
### Programming header (5 holes next to the CH330N)
|
||||
|
||||
Square pad = hole 1.
|
||||
|
||||
| Hole | Signal |
|
||||
|------|--------|
|
||||
| 1 | BOOT0 |
|
||||
| 2 | 3V3 |
|
||||
| 3 | GND |
|
||||
| 4 | SWDIO (PA13) |
|
||||
| 5 | SWCLK (PA14) |
|
||||
|
||||
The factory firmware repurposed PA13/PA14, so SWD only answered with **BOOT0
|
||||
tied high** (hole 1 → hole 2) at power-up. With our firmware this header is not
|
||||
needed at all — see flashing below.
|
||||
|
||||
### Flash readout protection
|
||||
|
||||
The STM32 was at **RDP level 1**: reads of flash are refused over both SWD and
|
||||
the ROM bootloader, so the factory firmware cannot be dumped. Level 1 (not the
|
||||
permanent level 2) still allows reflashing — unlocking does a mass-erase, which
|
||||
is what wiped the factory firmware. The only way past RDP1 to read it would be
|
||||
the F0 cold-boot stepping exploit (thousands of power cycles); not worth it.
|
||||
|
||||
## Firmware: two stages, so wire-free flashing can never break
|
||||
|
||||
The whole point of the design: **once installed, the dongle is reflashed over
|
||||
USB forever, with no BOOT0 wire and no ST-Link — and no firmware update can take
|
||||
that ability away.**
|
||||
|
||||
```
|
||||
flash (16 KB):
|
||||
0x08000000 +--------------------+ page 0
|
||||
| safeboot (4 KB) | stage 1: permanent recovery bootloader
|
||||
0x08001000 +--------------------+ page 4
|
||||
| app (12 KB) | stage 2: the radio firmware
|
||||
0x08004000 +--------------------+ page 16
|
||||
RAM (4 KB): 0x20000000 = magic word (app -> safeboot signal); .data/.bss from +4
|
||||
```
|
||||
|
||||
- **`safeboot`** (`firmware/safeboot.c`) runs first on every reset. It speaks a
|
||||
tiny, fixed-baud (115200 8N1) flash protocol over USB and can always erase and
|
||||
rewrite the app. It has no dependencies and is flashed once.
|
||||
- **`app`** (`firmware/app.c`) is the actual radio firmware. It is the only part
|
||||
future updates rewrite.
|
||||
- **The app updater only erases pages 4-15 — never safeboot.** So a buggy app
|
||||
can't remove the recovery path.
|
||||
|
||||
Recovery is guaranteed three ways, none needing the wire:
|
||||
1. normal update — the running app resets into safeboot on the `b` command (the
|
||||
CH330N keeps its USB power across an MCU reset, so the COM port doesn't drop);
|
||||
2. a corrupt/missing app — safeboot sees the app's reset vector is invalid and
|
||||
stays in flash mode;
|
||||
3. a booting-but-wedged app — safeboot opens a 1.5 s knock window on every cold
|
||||
boot; the updater spams `S` after a replug to catch it.
|
||||
|
||||
The only thing that could break it is corrupting safeboot itself, which the
|
||||
updater never writes.
|
||||
|
||||
### safeboot flash protocol (115200 8N1)
|
||||
|
||||
Each command replies one byte, `K` = ok / `E` = error, unless noted.
|
||||
|
||||
| Cmd | Bytes | Action |
|
||||
|-----|-------|--------|
|
||||
| `P` | | ping |
|
||||
| `V` | | prints `SBV1` (identify) |
|
||||
| `E` | | erase app region (pages 4-15) |
|
||||
| `W` | addr(LE32) n data[n] | program n bytes (n even, ≤128) |
|
||||
| `R` | addr(LE32) n | read n bytes (returns n bytes, no `K`) |
|
||||
| `G` | | `K`, then boot the app |
|
||||
|
||||
Banners: `SBF` = entered flash mode via the app; `SB1` = cold boot (knock
|
||||
window open); `SBR` = no valid app, staying in flash mode.
|
||||
|
||||
## Building
|
||||
|
||||
Needs the ARM toolchain PlatformIO ships:
|
||||
|
||||
```
|
||||
C:\Users\you\.platformio\penv\Scripts\pio.exe pkg install -g -p ststm32
|
||||
```
|
||||
|
||||
Then, from `firmware/`:
|
||||
|
||||
```
|
||||
.\build.ps1
|
||||
```
|
||||
|
||||
Produces `build/safeboot.bin`, `build/app.bin`, and `build/dongle_combined.bin`
|
||||
(safeboot padded to 4 KB + app), plus the sizes.
|
||||
|
||||
## Flashing
|
||||
|
||||
### One-time install (the only time the BOOT0 wire is used)
|
||||
|
||||
Puts safeboot + app on a stick that still has the factory (or any) firmware.
|
||||
|
||||
1. Wire **hole 1 → hole 2** (BOOT0 high) and plug the stick into USB.
|
||||
2. `python tools/dongle_flash.py firmware/build/dongle_combined.bin`
|
||||
- unlocks (mass-erase, the step that wipes the factory firmware),
|
||||
- writes and verifies the combined image,
|
||||
- starts it.
|
||||
3. Remove the wire. Never needed again.
|
||||
|
||||
`dongle_flash.py` drives the STM32 **ROM** UART bootloader (one-shot auto-baud
|
||||
per power-up, so power-cycle between attempts).
|
||||
|
||||
### App updates (normal, wire-free, forever after)
|
||||
|
||||
```
|
||||
python tools/safeboot_flash.py firmware/build/app.bin
|
||||
```
|
||||
|
||||
This resets the running app into safeboot and rewrites only the app region. If
|
||||
the app is dead or doesn't answer, start it with `--knock` and then replug the
|
||||
stick: it keeps knocking (reopening the port as it comes back) until it lands in
|
||||
safeboot's 1.5 s cold-boot window.
|
||||
|
||||
## Air settings
|
||||
|
||||
Matches `shared/HexapodProtocol/HexapodProtocol.h`: 2.4 GHz channel 96
|
||||
(2.496 GHz), 250 kbps, 5-byte address `HXPD1`, 16-bit CRC, auto-ack with
|
||||
dynamic payload lengths and ack payloads (protocol v4). The app sets these on
|
||||
the Si24R1 over SPI (same register layout as the nRF24L01+).
|
||||
|
||||
## Status / next
|
||||
|
||||
- [x] identify the hardware, map the radio + serial pins
|
||||
- [x] permanent two-stage firmware, wire-free USB flashing
|
||||
- [x] LED pin (PA0)
|
||||
- [x] bridge mode: the dongle stands in for the robot's radio, Unity runs the
|
||||
robot firmware (sim) behind it — see "Bridge mode" below
|
||||
- [ ] the other direction: Unity as the remote, driving the real robot
|
||||
- [ ] AT2401C pins: at desk range and even a few rooms away with the remote at
|
||||
minimum power, every on/off combination of PA1/PB1/PF0/PF1 received
|
||||
every packet, so they couldn't be told apart; the link works without
|
||||
driving them
|
||||
|
||||
## Bridge mode (Unity ↔ real remote)
|
||||
|
||||
The app listens on the hexapod's air settings as if it were the robot
|
||||
(ch 96, 250 kbps, `HXPD1`, auto-ack, dynamic payload lengths, ack payloads)
|
||||
and passes everything to the PC. Framing both ways over the USB serial
|
||||
(115200 8N1):
|
||||
|
||||
```
|
||||
A5 type len payload[len] sum sum = type + len + payload bytes (mod 256)
|
||||
```
|
||||
|
||||
| Direction | Type | Payload |
|
||||
|-----------|------|---------|
|
||||
| PC → dongle | `L` | — : start bridging (LED on) |
|
||||
| PC → dongle | `X` | — : stop |
|
||||
| PC → dongle | `A` | ≤ 32 bytes: the robot's `Telemetry`, sent back on the auto-ack of the remote's next packet (reloaded after every packet, like `robot/Comms.cpp`). Dropped if not refreshed for ~0.5 s, so the remote never shows stale telemetry as live |
|
||||
| dongle → PC | `R` | a packet the remote sent (its `CmdPacket`, any length; the firmware checks it) |
|
||||
| dongle → PC | `S` | once a "second": packets received (u16 LE), ack payloads loaded (u16 LE). Its second runs ~30% long (blocking UART sends miss SysTick ticks); it's only for display |
|
||||
|
||||
Plain text never contains `A5`, so the PC skips anything between frames;
|
||||
while bridging the app prints no text and only takes frames (and `b` for a
|
||||
reflash). USB serial input goes through DMA into a ring buffer (no
|
||||
interrupts in this app), and the USART's overrun stall is disabled, so a
|
||||
burst can't deafen it. Unity's side is
|
||||
`Hexapod_Simulation_Unity/Assets/HexapodSim/DongleLink.cs`.
|
||||
@@ -0,0 +1,356 @@
|
||||
/* Hexapod dongle - stage 2 "app" (bring-up / radio firmware).
|
||||
*
|
||||
* Links at 0x08001000 (after safeboot). Uses polling only (no interrupts), so
|
||||
* no Cortex-M0 vector relocation is needed - safeboot jumps straight to it.
|
||||
*
|
||||
* This v0 brings up the hardware and helps map the last unknown pins:
|
||||
* - USART1 PA9/PA10 115200 8N1 (USB serial via CH330N)
|
||||
* - SPI1 PA5/6/7, nRF24 CSN PA3 CE PA4 IRQ PA2
|
||||
* - free pins PA0 PA1 PB1 PF0 PF1 (LED + AT2401C control, still to identify)
|
||||
* Serial commands: v=info r=nRF regs 0-4=toggle a free pin b=reflash (safeboot)
|
||||
*/
|
||||
#include "common.h"
|
||||
|
||||
#define CSN_HI() BSRR(GPIOA) = (1u << 3)
|
||||
#define CSN_LO() BSRR(GPIOA) = (1u << (3 + 16))
|
||||
#define CE_HI() BSRR(GPIOA) = (1u << 4)
|
||||
#define CE_LO() BSRR(GPIOA) = (1u << (4 + 16))
|
||||
|
||||
static void spi_init(void)
|
||||
{
|
||||
RCC_AHBENR |= (1u << 17); /* GPIOA */
|
||||
RCC_APB2ENR |= (1u << 12); /* SPI1 */
|
||||
MODER(GPIOA) = (MODER(GPIOA) & ~((3u << 10) | (3u << 12) | (3u << 14)))
|
||||
| (2u << 10) | (2u << 12) | (2u << 14); /* PA5/6/7 AF0 */
|
||||
MODER(GPIOA) = (MODER(GPIOA) & ~((3u << 6) | (3u << 8)))
|
||||
| (1u << 6) | (1u << 8); /* PA3/PA4 out */
|
||||
MODER(GPIOA) &= ~(3u << 4); /* PA2 in */
|
||||
PUPDR(GPIOA) = (PUPDR(GPIOA) & ~(3u << 4)) | (1u << 4); /* PA2 pull-up */
|
||||
CSN_HI(); CE_LO();
|
||||
REG(0x40013000) = (1u << 2) | (1u << 9) | (1u << 8) | (2u << 3); /* CR1 */
|
||||
REG(0x40013004) = (7u << 8) | (1u << 12); /* CR2 8-bit */
|
||||
REG(0x40013000) |= (1u << 6); /* SPE */
|
||||
}
|
||||
static uint8_t spi_xfer(uint8_t b)
|
||||
{
|
||||
REG8(0x4001300C) = b;
|
||||
while (!(REG(0x40013008) & (1u << 0))) {}
|
||||
return REG8(0x4001300C);
|
||||
}
|
||||
static uint8_t nrf_read(uint8_t reg)
|
||||
{
|
||||
CSN_LO(); spi_xfer(reg & 0x1F); uint8_t v = spi_xfer(0xFF); CSN_HI();
|
||||
return v;
|
||||
}
|
||||
static void nrf_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
CSN_LO(); spi_xfer(0x20 | (reg & 0x1F)); spi_xfer(val); CSN_HI();
|
||||
}
|
||||
static void nrf_write_buf(uint8_t reg, const uint8_t *d, int n)
|
||||
{
|
||||
CSN_LO(); spi_xfer(0x20 | (reg & 0x1F)); while (n--) spi_xfer(*d++); CSN_HI();
|
||||
}
|
||||
static void nrf_cmd(uint8_t c) { CSN_LO(); spi_xfer(c); CSN_HI(); }
|
||||
|
||||
/* Receive on the hexapod protocol (ch 96, 250 kbps, addr HXPD1). Protocol v4
|
||||
* uses dynamic payload lengths (the remote's CmdPacket is 26 bytes, with ack
|
||||
* payloads), so this does too and accepts any length. Auto-ack is on, so the
|
||||
* remote's link % also shows whether our acks get back out (TX path). Lets us
|
||||
* find the AT2401C enable pins: packets only arrive when the LNA path is on. */
|
||||
static uint8_t rxmode;
|
||||
static uint16_t rx_count;
|
||||
static uint8_t rx_len;
|
||||
static void rx_setup(void)
|
||||
{
|
||||
static const uint8_t addr[5] = { 0x48, 0x58, 0x50, 0x44, 0x31 }; /* "HXPD1" */
|
||||
CE_LO();
|
||||
nrf_write(0x00, 0x0F); /* CONFIG: PWR_UP, PRIM_RX, EN_CRC, CRCO=16 */
|
||||
nrf_write(0x01, 0x01); /* EN_AA pipe0 */
|
||||
nrf_write(0x02, 0x01); /* EN_RXADDR pipe0 */
|
||||
nrf_write(0x03, 0x03); /* SETUP_AW = 5 bytes */
|
||||
nrf_write(0x05, 96); /* RF_CH */
|
||||
nrf_write(0x06, 0x26); /* RF_SETUP: 250 kbps, max power */
|
||||
nrf_write_buf(0x0A, addr, 5); /* RX_ADDR_P0 */
|
||||
nrf_write(0x1D, 0x04); /* FEATURE: EN_DPL */
|
||||
nrf_write(0x1C, 0x01); /* DYNPD: pipe0 */
|
||||
nrf_write(0x07, 0x70); /* clear status flags */
|
||||
nrf_cmd(0xE2); /* FLUSH_RX */
|
||||
rx_count = 0;
|
||||
rx_len = 0;
|
||||
CE_HI();
|
||||
}
|
||||
static void rx_poll(void)
|
||||
{
|
||||
while (nrf_read(0x07) & 0x40) { /* RX_DR */
|
||||
CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
|
||||
if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; } /* corrupt: flush */
|
||||
CSN_LO(); spi_xfer(0x61); for (int i = 0; i < n; i++) spi_xfer(0xFF); CSN_HI();
|
||||
nrf_write(0x07, 0x40); /* clear RX_DR */
|
||||
rx_len = n;
|
||||
rx_count++;
|
||||
}
|
||||
}
|
||||
|
||||
static const struct { uint32_t port; uint8_t pin; const char *name; } FREE[] = {
|
||||
{ GPIOA, 0, "PA0" }, { GPIOA, 1, "PA1" }, { GPIOB, 1, "PB1" },
|
||||
{ GPIOF, 0, "PF0" }, { GPIOF, 1, "PF1" },
|
||||
};
|
||||
#define NFREE 5
|
||||
static uint8_t fst[NFREE];
|
||||
static void free_init(void)
|
||||
{
|
||||
RCC_AHBENR |= (1u << 17) | (1u << 18) | (1u << 22);
|
||||
for (int i = 0; i < NFREE; i++) {
|
||||
MODER(FREE[i].port) = (MODER(FREE[i].port) & ~(3u << (FREE[i].pin * 2)))
|
||||
| (1u << (FREE[i].pin * 2));
|
||||
BSRR(FREE[i].port) = (1u << (FREE[i].pin + 16));
|
||||
}
|
||||
}
|
||||
|
||||
/* Every usable GPIO on the TSSOP-20 except PA9/PA10 (our serial). Sweep each
|
||||
* HIGH ~3 s so the user can watch the LED. Disturbs the radio; re-init after. */
|
||||
static const struct { uint32_t port; uint8_t pin; const char *name; } ALLPINS[] = {
|
||||
{ GPIOA, 0, "PA0" }, { GPIOA, 1, "PA1" }, { GPIOA, 2, "PA2 IRQ" },
|
||||
{ GPIOA, 3, "PA3 CSN" }, { GPIOA, 4, "PA4 CE" }, { GPIOA, 5, "PA5 SCK" },
|
||||
{ GPIOA, 6, "PA6 MISO" }, { GPIOA, 7, "PA7 MOSI" }, { GPIOB, 1, "PB1" },
|
||||
{ GPIOF, 0, "PF0" }, { GPIOF, 1, "PF1" }, { GPIOA, 13, "PA13 SWDIO" },
|
||||
{ GPIOA, 14, "PA14 SWCLK" },
|
||||
};
|
||||
static void sweep_all(void)
|
||||
{
|
||||
uart_puts("sweeping ALL pins, 3 s each (radio will be disturbed):\r\n");
|
||||
for (unsigned i = 0; i < sizeof(ALLPINS) / sizeof(ALLPINS[0]); i++) {
|
||||
uint32_t p = ALLPINS[i].port; uint8_t n = ALLPINS[i].pin;
|
||||
MODER(p) = (MODER(p) & ~(3u << (n * 2))) | (1u << (n * 2)); /* output */
|
||||
BSRR(p) = (1u << n); /* HIGH */
|
||||
uart_puts(">>> "); uart_puts(ALLPINS[i].name); uart_puts(" HIGH\r\n");
|
||||
delay_ms(3000);
|
||||
BSRR(p) = (1u << (n + 16)); /* LOW */
|
||||
delay_ms(500);
|
||||
}
|
||||
uart_puts("sweep done; restoring radio\r\n");
|
||||
spi_init();
|
||||
nrf_write(0, 0x0E);
|
||||
}
|
||||
|
||||
static void hex2(uint8_t b) { const char *h = "0123456789ABCDEF"; uart_putc(h[b >> 4]); uart_putc(h[b & 0xF]); }
|
||||
static void banner(void)
|
||||
{
|
||||
uart_puts("\r\nhexapod dongle app v0 (@0x08001000)\r\n");
|
||||
uart_puts("nRF CONFIG=0x"); hex2(nrf_read(0)); uart_puts(" STATUS=0x"); hex2(nrf_read(7));
|
||||
uart_puts(" RF_CH=0x"); hex2(nrf_read(5)); uart_puts(" RF_SETUP=0x"); hex2(nrf_read(6));
|
||||
uart_puts("\r\ncmds: v r 0-4(PA0/PA1/PB1/PF0/PF1) x=rx test b=reflash; bridge: frame A5 'L'\r\n");
|
||||
}
|
||||
static void regdump(void)
|
||||
{
|
||||
static const uint8_t rg[] = {0,1,2,3,4,5,6,7,8,9,0x17};
|
||||
for (unsigned i = 0; i < sizeof(rg); i++) { uart_puts(" R"); hex2(rg[i]); uart_putc('='); hex2(nrf_read(rg[i])); }
|
||||
uart_puts("\r\n");
|
||||
}
|
||||
/* Pin finder: sit in RX on a WiFi-busy channel and report the carrier-detect
|
||||
* (RPD) bit. Toggling the real RXEN pin switches the AT2401C LNA on, which lifts
|
||||
* ambient 2.4 GHz above the -64 dBm detect threshold -> RPD flips to 1. */
|
||||
static uint8_t finder;
|
||||
static uint8_t finder_ch = 62; /* 2462 MHz ~ WiFi ch 11 */
|
||||
static void finder_rx(void)
|
||||
{
|
||||
CE_LO();
|
||||
nrf_write(0x05, finder_ch); /* RF_CH */
|
||||
nrf_write(0x06, 0x0F); /* RF_SETUP: keep it simple */
|
||||
nrf_write(0x00, 0x0F); /* CONFIG: PWR_UP, PRIM_RX, CRC16 */
|
||||
delay_ms(2);
|
||||
CE_HI(); /* enter RX */
|
||||
delay_ms(2);
|
||||
}
|
||||
|
||||
/* ---- USART1 RX through circular DMA ------------------------------------
|
||||
* The bridge moves ~100 frames/s each way at 115200 baud. Polled single-byte
|
||||
* RX loses bytes while we block on a TX frame or an SPI read, and this M0 app
|
||||
* runs without interrupts (no vector relocation), so DMA channel 3 (USART1_RX)
|
||||
* writes every byte into a ring we drain at leisure. */
|
||||
#define DMA_CCR3 REG(0x40020030)
|
||||
#define DMA_CNDTR3 REG(0x40020034)
|
||||
#define DMA_CPAR3 REG(0x40020038)
|
||||
#define DMA_CMAR3 REG(0x4002003C)
|
||||
#define USART1_CR3 REG(0x40013808)
|
||||
#define RXRING 256u
|
||||
static volatile uint8_t rxring[RXRING];
|
||||
static uint16_t rxtail;
|
||||
static void uart_dma_init(void)
|
||||
{
|
||||
RCC_AHBENR |= 1u; /* DMA1 */
|
||||
while (!(USART1_ISR & (1u << 6))) {} /* let any TX finish */
|
||||
USART1_CR1 &= ~1u; /* UE off: CR3 OVRDIS needs it */
|
||||
USART1_CR3 = (1u << 6) | (1u << 12); /* DMAR, OVRDIS (never stall on overrun) */
|
||||
USART1_CR1 |= 1u;
|
||||
DMA_CCR3 = 0;
|
||||
DMA_CPAR3 = 0x40013824u; /* USART1_RDR */
|
||||
DMA_CMAR3 = (uint32_t)rxring;
|
||||
DMA_CNDTR3 = RXRING;
|
||||
DMA_CCR3 = (1u << 7) | (1u << 5) | 1u; /* MINC, CIRC, EN; 8-bit, periph -> mem */
|
||||
}
|
||||
static int rx_getc(void)
|
||||
{
|
||||
uint16_t head = (uint16_t)(RXRING - DMA_CNDTR3) % RXRING;
|
||||
if (head == rxtail) return -1;
|
||||
uint8_t c = rxring[rxtail];
|
||||
rxtail = (uint16_t)((rxtail + 1) % RXRING);
|
||||
return c;
|
||||
}
|
||||
|
||||
/* ---- Bridge: the dongle stands in for the robot's radio ------------------
|
||||
* Framing both ways: A5 type len payload[len] sum, sum = type+len+payload (mod 256).
|
||||
* host -> dongle 'L' listen (enter bridge) 'X' leave
|
||||
* 'A' ack payload (<= 32 B): goes back on the auto-ack of the
|
||||
* next packet, like the robot's Comms::loadAckPayload.
|
||||
* Dropped if the host stops refreshing it for 500 ms, so
|
||||
* the remote doesn't see stale telemetry as live.
|
||||
* dongle -> host 'R' a received packet (the remote's CmdPacket)
|
||||
* 'S' once a second: packets received (u16 LE), acks loaded (u16 LE)
|
||||
* Plain text never contains A5, so the host can skip anything between frames. */
|
||||
static uint8_t bridge;
|
||||
static uint8_t ack_buf[32], ack_len;
|
||||
static uint32_t ack_ms;
|
||||
static uint16_t br_rx, br_acks;
|
||||
static void frame_send(uint8_t type, const uint8_t *d, uint8_t n)
|
||||
{
|
||||
uint8_t sum = (uint8_t)(type + n);
|
||||
uart_putc((char)0xA5); uart_putc((char)type); uart_putc((char)n);
|
||||
for (uint8_t i = 0; i < n; i++) { uart_putc((char)d[i]); sum = (uint8_t)(sum + d[i]); }
|
||||
uart_putc((char)sum);
|
||||
}
|
||||
static void ack_load(void)
|
||||
{
|
||||
nrf_cmd(0xE1); /* FLUSH_TX: only the newest one */
|
||||
if (!ack_len) return;
|
||||
CSN_LO(); spi_xfer(0xA8); for (uint8_t i = 0; i < ack_len; i++) spi_xfer(ack_buf[i]); CSN_HI(); /* W_ACK_PAYLOAD pipe 0 */
|
||||
br_acks++;
|
||||
}
|
||||
static void bridge_start(void)
|
||||
{
|
||||
rx_setup(); /* ch96 / 250k / HXPD1 / auto-ack / dynamic length */
|
||||
CE_LO();
|
||||
nrf_write(0x1D, 0x06); /* FEATURE: EN_DPL | EN_ACK_PAY */
|
||||
nrf_write(0x1C, 0x01); /* DYNPD pipe 0 */
|
||||
nrf_cmd(0xE1);
|
||||
ack_len = 0; br_rx = br_acks = 0;
|
||||
rxmode = 0; finder = 0; bridge = 1;
|
||||
BSRR(GPIOA) = 1u; /* LED on while bridging */
|
||||
CE_HI();
|
||||
}
|
||||
static void bridge_stop(void)
|
||||
{
|
||||
bridge = 0; ack_len = 0;
|
||||
CE_LO(); nrf_write(0, 0x0E); nrf_cmd(0xE1);
|
||||
BSRR(GPIOA) = 1u << 16;
|
||||
}
|
||||
static void bridge_poll(uint32_t now)
|
||||
{
|
||||
if (ack_len && now - ack_ms > 500) { ack_len = 0; nrf_cmd(0xE1); } /* host went quiet */
|
||||
while (nrf_read(0x07) & 0x40) { /* RX_DR */
|
||||
uint8_t buf[32];
|
||||
CSN_LO(); spi_xfer(0x60); uint8_t n = spi_xfer(0xFF); CSN_HI(); /* R_RX_PL_WID */
|
||||
if (n > 32) { nrf_cmd(0xE2); nrf_write(0x07, 0x40); break; }
|
||||
CSN_LO(); spi_xfer(0x61); for (uint8_t i = 0; i < n; i++) buf[i] = spi_xfer(0xFF); CSN_HI();
|
||||
nrf_write(0x07, 0x40);
|
||||
ack_load(); /* the ack for the NEXT packet */
|
||||
frame_send('R', buf, n);
|
||||
br_rx++;
|
||||
if ((br_rx & 31) == 0) BSRR(GPIOA) = (IDR(GPIOA) & 1u) ? (1u << 16) : 1u; /* LED flicker */
|
||||
}
|
||||
}
|
||||
/* Host frame parser; returns 1 while inside a frame (so its bytes aren't
|
||||
* taken as single-letter commands). */
|
||||
static uint8_t fr_state, fr_type, fr_len, fr_got, fr_sum, fr_buf[32];
|
||||
static int frame_rx(int c, uint32_t now)
|
||||
{
|
||||
/* if-chain, not switch: a switch becomes a jump table needing libgcc */
|
||||
if (fr_state == 0) {
|
||||
if (c != 0xA5) return 0;
|
||||
fr_state = 1;
|
||||
} else if (fr_state == 1) {
|
||||
fr_type = (uint8_t)c;
|
||||
fr_state = 2;
|
||||
} else if (fr_state == 2) {
|
||||
fr_len = (uint8_t)c; fr_got = 0; fr_sum = (uint8_t)(fr_type + fr_len);
|
||||
fr_state = (fr_len > 32) ? 0 : (fr_len ? 3 : 4);
|
||||
} else if (fr_state == 3) {
|
||||
fr_buf[fr_got++] = (uint8_t)c; fr_sum = (uint8_t)(fr_sum + c);
|
||||
if (fr_got == fr_len) fr_state = 4;
|
||||
} else {
|
||||
fr_state = 0;
|
||||
if ((uint8_t)c != fr_sum) return 1; /* corrupt: drop */
|
||||
if (fr_type == 'L') bridge_start();
|
||||
else if (fr_type == 'X') bridge_stop();
|
||||
else if (fr_type == 'A' && bridge) {
|
||||
for (uint8_t i = 0; i < fr_len; i++) ack_buf[i] = fr_buf[i];
|
||||
ack_len = fr_len; ack_ms = now;
|
||||
ack_load();
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void reflash(void) /* ask safeboot to take over, no wire needed */
|
||||
{
|
||||
uart_puts("resetting into safeboot flash mode...\r\n");
|
||||
while (!(USART1_ISR & (1u << 6))) {} /* TC */
|
||||
REG(MAGIC_ADDR) = MAGIC_FLASH;
|
||||
REG(0xE000ED0C) = 0x05FA0004; /* SCB AIRCR SYSRESETREQ */
|
||||
for (;;) {}
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
uart_init8n1();
|
||||
uart_dma_init();
|
||||
spi_init();
|
||||
free_init();
|
||||
nrf_write(0, 0x0E);
|
||||
delay_ms(5);
|
||||
banner();
|
||||
|
||||
uint32_t now = 0, t = 0;
|
||||
systick_setup();
|
||||
for (;;) {
|
||||
uint32_t ctrl = STK_CTRL; /* one read: reading clears COUNTFLAG */
|
||||
if (ctrl == 0) systick_setup(); /* delay_ms() (sweep, finder) stops it */
|
||||
else if (ctrl & 0x10000) { now++; t++; } /* 1 ms ticks, polled */
|
||||
if (bridge) bridge_poll(now);
|
||||
int c = rx_getc();
|
||||
if (c >= 0 && frame_rx(c, now)) c = -1; /* consumed by a host frame */
|
||||
if (c < 0) {}
|
||||
else if (bridge && c != 'b' && c != 'B') {} /* bridging: only frames (and reflash) */
|
||||
else if (c == 'v' || c == 'V') banner();
|
||||
else if (c == 'r' || c == 'R') regdump();
|
||||
else if (c == 'b' || c == 'B') reflash();
|
||||
else if (c == 'x' || c == 'X') { rxmode = !rxmode; finder = 0; if (rxmode) rx_setup(); else { CE_LO(); nrf_write(0, 0x0E); } uart_puts(rxmode ? "RX mode on (ch96/250k/HXPD1)\r\n" : "RX off\r\n"); }
|
||||
else if (c == 'a' || c == 'A') sweep_all();
|
||||
else if (c == 'f' || c == 'F') { finder = !finder; if (finder) finder_rx(); else { CE_LO(); nrf_write(0, 0x0E); } uart_puts(finder ? "finder RX on\r\n" : "finder off\r\n"); }
|
||||
else if (c == 'n' || c == 'N') { finder_ch = (finder_ch == 62) ? 37 : (finder_ch == 37 ? 12 : 62); if (finder) finder_rx(); uart_puts("ch="); hex2(finder_ch); uart_puts("\r\n"); }
|
||||
else if (c >= '0' && c <= '4') {
|
||||
int i = c - '0';
|
||||
fst[i] = !fst[i];
|
||||
BSRR(FREE[i].port) = fst[i] ? (1u << FREE[i].pin) : (1u << (FREE[i].pin + 16));
|
||||
uart_puts("pin "); uart_puts(FREE[i].name); uart_puts(fst[i] ? " HIGH" : " LOW");
|
||||
if (finder) { delay_ms(2); uart_puts(" RPD="); hex2(nrf_read(9)); }
|
||||
uart_puts("\r\n");
|
||||
}
|
||||
if (rxmode) rx_poll();
|
||||
if (t >= 1000) {
|
||||
t = 0;
|
||||
if (bridge) {
|
||||
uint8_t s[4] = { (uint8_t)br_rx, (uint8_t)(br_rx >> 8), (uint8_t)br_acks, (uint8_t)(br_acks >> 8) };
|
||||
frame_send('S', s, 4);
|
||||
br_rx = br_acks = 0;
|
||||
}
|
||||
else if (rxmode) { uart_puts("RX pkts="); hex2(rx_count >> 8); hex2(rx_count & 0xFF);
|
||||
uart_puts(" len="); hex2(rx_len);
|
||||
uart_puts(" pins="); for (int i = 0; i < NFREE; i++) uart_putc(fst[i] ? '1' : '0');
|
||||
uart_puts(" (0=PA0LED,1=PA1,2=PB1,3=PF0,4=PF1)\r\n"); rx_count = 0; }
|
||||
else if (finder) { uart_puts("finder ch=0x"); hex2(finder_ch); uart_puts(" RPD="); hex2(nrf_read(9));
|
||||
uart_puts(" pins="); for (int i = 0; i < NFREE; i++) uart_putc(fst[i] ? '1' : '0'); uart_puts("\r\n"); }
|
||||
else { uart_puts("hb STATUS=0x"); hex2(nrf_read(7)); uart_puts("\r\n"); }
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
/* app: flash pages 4-15 (12 KB) after safeboot. RAM starts at +4 so the magic
|
||||
* word at 0x20000000 (used to signal safeboot) is left alone. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08001000, LENGTH = 12K
|
||||
RAM (rwx) : ORIGIN = 0x20000004, LENGTH = 4092
|
||||
}
|
||||
_estack = 0x20001000;
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.isr_vector : { KEEP(*(.isr_vector)) } > FLASH
|
||||
.text : { *(.text*) *(.rodata*) . = ALIGN(4); } > FLASH
|
||||
_sidata = LOADADDR(.data);
|
||||
.data : { . = ALIGN(4); _sdata = .; *(.data*) . = ALIGN(4); _edata = .; } > RAM AT > FLASH
|
||||
.bss : { . = ALIGN(4); _sbss = .; *(.bss*) *(COMMON) . = ALIGN(4); _ebss = .; } > RAM
|
||||
/DISCARD/ : { *(.ARM.exidx*) *(.ARM.extab*) }
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
/* Startup for the stage-2 app. Polling only, so no IRQ vectors are needed
|
||||
* beyond SP/Reset; safeboot jumps here directly (no M0 vector relocation). */
|
||||
#include <stdint.h>
|
||||
|
||||
extern uint32_t _sidata, _sdata, _edata, _sbss, _ebss, _estack;
|
||||
int main(void);
|
||||
|
||||
void Reset_Handler(void)
|
||||
{
|
||||
uint32_t *src = &_sidata, *dst = &_sdata;
|
||||
while (dst < &_edata) *dst++ = *src++;
|
||||
for (dst = &_sbss; dst < &_ebss;) *dst++ = 0;
|
||||
main();
|
||||
for (;;) {}
|
||||
}
|
||||
void Default_Handler(void) { for (;;) {} }
|
||||
|
||||
__attribute__((section(".isr_vector"), used))
|
||||
void (*const g_vectors[])(void) = {
|
||||
(void (*)(void))&_estack, Reset_Handler, Default_Handler, Default_Handler,
|
||||
0, 0, 0, 0, 0, 0, 0, Default_Handler, 0, 0, Default_Handler, Default_Handler,
|
||||
};
|
||||
@@ -0,0 +1,36 @@
|
||||
# Build the dongle firmware: safeboot.bin (stage 1), app.bin (stage 2), and
|
||||
# dongle_combined.bin (safeboot padded to 4 KB + app) for the one-time install.
|
||||
# Uses the ARM toolchain PlatformIO installed (pio pkg install -g -p ststm32).
|
||||
$ErrorActionPreference = "Stop"
|
||||
$bin = "$env:USERPROFILE\.platformio\packages\toolchain-gccarmnoneeabi\bin"
|
||||
$gcc = (Get-ChildItem "$bin\arm-none-eabi-gcc*.exe" | Select-Object -First 1).FullName
|
||||
$objcopy = (Get-ChildItem "$bin\arm-none-eabi-objcopy*.exe" | Select-Object -First 1).FullName
|
||||
$size = (Get-ChildItem "$bin\arm-none-eabi-size*.exe" | Select-Object -First 1).FullName
|
||||
$cf = "-mcpu=cortex-m0 -mthumb -Os -ffreestanding -nostdlib -ffunction-sections -fdata-sections -Wall -Wextra"
|
||||
$here = $PSScriptRoot
|
||||
Set-Location $here
|
||||
$out = "$here\build"; New-Item -ItemType Directory -Force $out | Out-Null
|
||||
|
||||
$lf = @("-mcpu=cortex-m0", "-mthumb", "-nostdlib", "-Wl,--gc-sections")
|
||||
# stage 1
|
||||
& $gcc $cf.Split(' ') -c safeboot.c -o "$out\safeboot.o"
|
||||
& $gcc $lf -T safeboot.ld "$out\safeboot.o" -o "$out\safeboot.elf"
|
||||
& $objcopy -O binary "$out\safeboot.elf" "$out\safeboot.bin"
|
||||
# stage 2
|
||||
& $gcc $cf.Split(' ') -c app.c -o "$out\app.o"
|
||||
& $gcc $cf.Split(' ') -c app_startup.c -o "$out\app_startup.o"
|
||||
& $gcc $lf -T app.ld "$out\app.o" "$out\app_startup.o" -o "$out\app.elf"
|
||||
& $objcopy -O binary "$out\app.elf" "$out\app.bin"
|
||||
|
||||
& $size "$out\safeboot.elf" "$out\app.elf"
|
||||
|
||||
# combined image: safeboot in [0,4KB), app at 4KB
|
||||
$sb = [System.IO.File]::ReadAllBytes("$out\safeboot.bin")
|
||||
$ap = [System.IO.File]::ReadAllBytes("$out\app.bin")
|
||||
if ($sb.Length -gt 4096) { throw "safeboot is $($sb.Length) bytes, over the 4 KB reserved" }
|
||||
$img = New-Object byte[] (4096 + $ap.Length)
|
||||
for ($i = 0; $i -lt $img.Length; $i++) { $img[$i] = 0xFF }
|
||||
[Array]::Copy($sb, 0, $img, 0, $sb.Length)
|
||||
[Array]::Copy($ap, 0, $img, 4096, $ap.Length)
|
||||
[System.IO.File]::WriteAllBytes("$out\dongle_combined.bin", $img)
|
||||
Write-Host "safeboot $($sb.Length) B, app $($ap.Length) B, combined $($img.Length) B -> build\dongle_combined.bin"
|
||||
@@ -0,0 +1,90 @@
|
||||
/* Shared register defs + helpers for the hexapod dongle (STM32F030F4 @ 8 MHz).
|
||||
* Magic word at 0x20000000 survives a soft reset: the app sets it to ask
|
||||
* safeboot to enter flash mode. Both linker scripts keep RAM clear of it
|
||||
* (RAM ORIGIN 0x20000004). */
|
||||
#ifndef DONGLE_COMMON_H
|
||||
#define DONGLE_COMMON_H
|
||||
#include <stdint.h>
|
||||
|
||||
#define REG(a) (*(volatile uint32_t *)(a))
|
||||
#define REG8(a) (*(volatile uint8_t *)(a))
|
||||
|
||||
#define MAGIC_ADDR 0x20000000u
|
||||
#define MAGIC_FLASH 0xB00710ADu /* "BOOT LOAD": app -> safeboot flash */
|
||||
|
||||
#define APP_BASE 0x08001000u /* stage 2 starts here (page 4) */
|
||||
#define APP_PAGE_LO 4
|
||||
#define APP_PAGE_HI 15 /* 16 KB / 1 KB pages = 0..15 */
|
||||
#define PAGE_SIZE 1024u
|
||||
|
||||
/* RCC */
|
||||
#define RCC_AHBENR REG(0x40021014)
|
||||
#define RCC_APB2ENR REG(0x40021018)
|
||||
#define RCC_APB2RSTR REG(0x4002100C)
|
||||
#define RCC_AHBRSTR REG(0x40021028)
|
||||
/* GPIO */
|
||||
#define GPIOA 0x48000000u
|
||||
#define GPIOB 0x48000400u
|
||||
#define GPIOF 0x48001400u
|
||||
#define MODER(p) REG((p)+0x00)
|
||||
#define OTYPER(p) REG((p)+0x04)
|
||||
#define PUPDR(p) REG((p)+0x0C)
|
||||
#define IDR(p) REG((p)+0x10)
|
||||
#define BSRR(p) REG((p)+0x18)
|
||||
#define AFRH(p) REG((p)+0x24)
|
||||
/* USART1 */
|
||||
#define USART1_CR1 REG(0x40013800)
|
||||
#define USART1_BRR REG(0x4001380C)
|
||||
#define USART1_ISR REG(0x4001381C)
|
||||
#define USART1_ICR REG(0x40013820)
|
||||
#define USART1_RDR REG(0x40013824)
|
||||
#define USART1_TDR REG(0x40013828)
|
||||
/* SysTick */
|
||||
#define STK_CTRL REG(0xE000E010)
|
||||
#define STK_LOAD REG(0xE000E014)
|
||||
#define STK_VAL REG(0xE000E018)
|
||||
/* FLASH */
|
||||
#define FLASH_KEYR REG(0x40022004)
|
||||
#define FLASH_SR REG(0x4002200C)
|
||||
#define FLASH_CR REG(0x40022010)
|
||||
#define FLASH_AR REG(0x40022014)
|
||||
|
||||
static inline void uart_init8n1(void) /* USART1 PA9/PA10, 115200 8N1, 8 MHz */
|
||||
{
|
||||
RCC_AHBENR |= (1u << 17); /* GPIOA */
|
||||
RCC_APB2ENR |= (1u << 14); /* USART1 */
|
||||
MODER(GPIOA) = (MODER(GPIOA) & ~((3u << 18) | (3u << 20)))
|
||||
| (2u << 18) | (2u << 20);
|
||||
AFRH(GPIOA) = (AFRH(GPIOA) & ~((0xFu << 4) | (0xFu << 8)))
|
||||
| (1u << 4) | (1u << 8);
|
||||
USART1_BRR = 69;
|
||||
USART1_CR1 = (1u << 0) | (1u << 2) | (1u << 3);
|
||||
}
|
||||
static inline void uart_putc(char c)
|
||||
{
|
||||
while (!(USART1_ISR & (1u << 7))) {}
|
||||
USART1_TDR = (uint8_t)c;
|
||||
}
|
||||
static inline void uart_puts(const char *s) { while (*s) uart_putc(*s++); }
|
||||
/* Clear overrun / noise / framing / parity errors. While ORE is set the USART
|
||||
* stops moving new bytes into RDR, so one burst the app didn't read in time
|
||||
* (e.g. SignalRGB probing the COM port) used to deafen it until a replug. */
|
||||
static inline void uart_clear_errors(void)
|
||||
{
|
||||
if (USART1_ISR & 0x0Fu) USART1_ICR = 0x0Fu; /* ORECF NCF FECF PECF */
|
||||
}
|
||||
static inline int uart_getc(void) /* non-blocking, -1 if none */
|
||||
{
|
||||
uart_clear_errors();
|
||||
if (USART1_ISR & (1u << 5)) return (uint8_t)USART1_RDR;
|
||||
return -1;
|
||||
}
|
||||
static inline uint8_t uart_getc_block(void)
|
||||
{
|
||||
while (!(USART1_ISR & (1u << 5))) uart_clear_errors();
|
||||
return (uint8_t)USART1_RDR;
|
||||
}
|
||||
static inline void systick_setup(void) { STK_LOAD = 8000 - 1; STK_VAL = 0; STK_CTRL = 5; }
|
||||
static inline void delay_ms(uint32_t ms) { systick_setup(); while (ms--) { while (!(STK_CTRL & 0x10000)) {} } STK_CTRL = 0; }
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,154 @@
|
||||
/* Hexapod dongle - stage 1 "safeboot" (permanent recovery bootloader).
|
||||
*
|
||||
* Lives in the first 4 KB of flash (pages 0-3) and runs first on every reset.
|
||||
* It speaks a tiny fixed-baud (115200 8N1) flashing protocol over USB and can
|
||||
* always erase/rewrite the application (pages 4-15). It is flashed ONCE and the
|
||||
* host flasher never erases it, so no application update can remove the
|
||||
* wire-free recovery path. See ../README.md.
|
||||
*
|
||||
* Entry decisions:
|
||||
* - soft reset with MAGIC_FLASH set -> enter flash mode immediately
|
||||
* - else print banner, open a knock window; host 'S' -> flash mode
|
||||
* - else if the app looks valid -> jump to it
|
||||
* - else (no valid app) -> stay in flash mode (recovery)
|
||||
*
|
||||
* Flash-mode protocol (host -> safeboot, each replies one byte 'K' ok / 'E' err
|
||||
* unless noted):
|
||||
* 'P' ping -> 'K'
|
||||
* 'E' erase app region (pages 4-15)
|
||||
* 'W' a0 a1 a2 a3 n d[n] write n bytes (n even, <=128) at little-endian addr
|
||||
* 'R' a0 a1 a2 a3 n read n bytes at addr -> sends n bytes (no K)
|
||||
* 'G' 'K', then boot the app (or reset)
|
||||
* 'V' -> prints "SBV1\n" (identify)
|
||||
*/
|
||||
#include "common.h"
|
||||
|
||||
#define KNOCK_MS 1500
|
||||
|
||||
/* ---- startup (custom: read the magic word before touching RAM) ---- */
|
||||
extern uint32_t _sidata, _sdata, _edata, _sbss, _ebss, _estack;
|
||||
int sb_main(int forced);
|
||||
|
||||
void Reset_Handler(void)
|
||||
{
|
||||
uint32_t m = REG(MAGIC_ADDR);
|
||||
REG(MAGIC_ADDR) = 0;
|
||||
uint32_t *src = &_sidata, *dst = &_sdata;
|
||||
while (dst < &_edata) *dst++ = *src++;
|
||||
for (dst = &_sbss; dst < &_ebss;) *dst++ = 0;
|
||||
sb_main(m == MAGIC_FLASH);
|
||||
for (;;) {}
|
||||
}
|
||||
void Default_Handler(void) { for (;;) {} }
|
||||
__attribute__((section(".isr_vector"), used))
|
||||
void (*const g_vectors[])(void) = {
|
||||
(void (*)(void))&_estack, Reset_Handler, Default_Handler, Default_Handler,
|
||||
0, 0, 0, 0, 0, 0, 0, Default_Handler, 0, 0, Default_Handler, Default_Handler,
|
||||
};
|
||||
|
||||
/* ---- flash programming ---- */
|
||||
static void flash_unlock(void)
|
||||
{
|
||||
if (FLASH_CR & (1u << 7)) { FLASH_KEYR = 0x45670123; FLASH_KEYR = 0xCDEF89AB; }
|
||||
}
|
||||
static int flash_wait(void)
|
||||
{
|
||||
while (FLASH_SR & (1u << 0)) {} /* BSY */
|
||||
uint32_t sr = FLASH_SR;
|
||||
FLASH_SR = (1u << 5) | (1u << 2) | (1u << 4); /* clear EOP/PGERR/WRPRT */
|
||||
return !(sr & ((1u << 2) | (1u << 4))); /* ok if no error bits */
|
||||
}
|
||||
static int flash_erase_page(uint32_t addr)
|
||||
{
|
||||
flash_wait();
|
||||
FLASH_CR |= (1u << 1); /* PER */
|
||||
FLASH_AR = addr;
|
||||
FLASH_CR |= (1u << 6); /* STRT */
|
||||
int ok = flash_wait();
|
||||
FLASH_CR &= ~(1u << 1);
|
||||
return ok;
|
||||
}
|
||||
static int flash_prog_hw(uint32_t addr, uint16_t hw)
|
||||
{
|
||||
flash_wait();
|
||||
FLASH_CR |= (1u << 0); /* PG */
|
||||
*(volatile uint16_t *)addr = hw;
|
||||
int ok = flash_wait();
|
||||
FLASH_CR &= ~(1u << 0);
|
||||
return ok && (*(volatile uint16_t *)addr == hw);
|
||||
}
|
||||
static int erase_app(void)
|
||||
{
|
||||
flash_unlock();
|
||||
for (int p = APP_PAGE_LO; p <= APP_PAGE_HI; p++)
|
||||
if (!flash_erase_page(0x08000000u + p * PAGE_SIZE)) return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static uint32_t rd4(void)
|
||||
{
|
||||
uint32_t v = uart_getc_block();
|
||||
v |= (uint32_t)uart_getc_block() << 8;
|
||||
v |= (uint32_t)uart_getc_block() << 16;
|
||||
v |= (uint32_t)uart_getc_block() << 24;
|
||||
return v;
|
||||
}
|
||||
|
||||
static void boot_app(void)
|
||||
{
|
||||
uint32_t sp = *(volatile uint32_t *)APP_BASE;
|
||||
uint32_t pc = *(volatile uint32_t *)(APP_BASE + 4);
|
||||
if (sp >= 0x20000000u && sp <= 0x20001000u) {
|
||||
__asm volatile("msr msp, %0" : : "r"(sp));
|
||||
((void (*)(void))pc)();
|
||||
}
|
||||
}
|
||||
|
||||
static void flash_loop(void)
|
||||
{
|
||||
flash_unlock();
|
||||
for (;;) {
|
||||
int c = uart_getc_block();
|
||||
if (c == 'P') uart_putc('K');
|
||||
else if (c == 'V') uart_puts("SBV1\r\n");
|
||||
else if (c == 'E') uart_putc(erase_app() ? 'K' : 'E');
|
||||
else if (c == 'W') {
|
||||
uint32_t a = rd4();
|
||||
uint8_t n = uart_getc_block();
|
||||
uint8_t buf[128];
|
||||
for (uint8_t i = 0; i < n; i++) buf[i] = uart_getc_block();
|
||||
int ok = (n <= 128) && !(n & 1) && a >= APP_BASE && (a + n) <= 0x08004000u;
|
||||
for (uint8_t i = 0; ok && i < n; i += 2)
|
||||
ok = flash_prog_hw(a + i, (uint16_t)(buf[i] | (buf[i + 1] << 8)));
|
||||
uart_putc(ok ? 'K' : 'E');
|
||||
}
|
||||
else if (c == 'R') {
|
||||
uint32_t a = rd4();
|
||||
uint8_t n = uart_getc_block();
|
||||
for (uint8_t i = 0; i < n; i++) uart_putc(*(volatile uint8_t *)(a + i));
|
||||
}
|
||||
else if (c == 'G') { uart_putc('K'); boot_app(); /* if it returns, stay */ }
|
||||
}
|
||||
}
|
||||
|
||||
int sb_main(int forced)
|
||||
{
|
||||
uart_init8n1();
|
||||
|
||||
if (forced) { uart_puts("SBF\r\n"); flash_loop(); }
|
||||
|
||||
uart_puts("SB1\r\n"); /* cold boot: offer a knock window */
|
||||
systick_setup();
|
||||
uint32_t ms = 0;
|
||||
while (ms < KNOCK_MS) {
|
||||
int c = uart_getc();
|
||||
if (c == 'S') { uart_putc('K'); flash_loop(); }
|
||||
if (STK_CTRL & 0x10000) ms++;
|
||||
}
|
||||
STK_CTRL = 0;
|
||||
|
||||
boot_app(); /* valid app -> never returns */
|
||||
uart_puts("SBR\r\n"); /* no app -> recovery flash mode */
|
||||
flash_loop();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
/* safeboot: first 4 KB of flash. RAM starts at +4 so the magic word at
|
||||
* 0x20000000 is never touched by .data/.bss. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 4K
|
||||
RAM (rwx) : ORIGIN = 0x20000004, LENGTH = 4092
|
||||
}
|
||||
_estack = 0x20001000;
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.isr_vector : { KEEP(*(.isr_vector)) } > FLASH
|
||||
.text : { *(.text*) *(.rodata*) . = ALIGN(4); } > FLASH
|
||||
_sidata = LOADADDR(.data);
|
||||
.data : { . = ALIGN(4); _sdata = .; *(.data*) . = ALIGN(4); _edata = .; } > RAM AT > FLASH
|
||||
.bss : { . = ALIGN(4); _sbss = .; *(.bss*) *(COMMON) . = ALIGN(4); _ebss = .; } > RAM
|
||||
/DISCARD/ : { *(.ARM.exidx*) *(.ARM.extab*) }
|
||||
}
|
||||
@@ -0,0 +1,191 @@
|
||||
"""Flash a .bin to the dongle's STM32F030 over the ROM UART bootloader (COM16).
|
||||
First run unlocks (readout-unprotect = mass erase + drop RDP), then writes & verifies.
|
||||
usage: dongle_flash.py firmware.bin [--port COM16]"""
|
||||
import sys, time, serial
|
||||
|
||||
ACK, NACK = 0x79, 0x1F
|
||||
FLASH = 0x08000000
|
||||
|
||||
|
||||
def open_racing(port, open_timeout=20.0):
|
||||
"""Keep retrying to open the port - wins the race against programs (e.g.
|
||||
SignalRGB) that also grab CH340 COM ports on enumeration."""
|
||||
t = time.time()
|
||||
while True:
|
||||
try:
|
||||
return serial.Serial(port, 115200, parity=serial.PARITY_EVEN,
|
||||
stopbits=1, bytesize=8, timeout=1.0)
|
||||
except serial.SerialException:
|
||||
if time.time() - t > open_timeout:
|
||||
raise
|
||||
time.sleep(0.02)
|
||||
|
||||
|
||||
class BL:
|
||||
def __init__(self, port):
|
||||
self.port = port
|
||||
self.s = open_racing(port)
|
||||
|
||||
def reopen(self):
|
||||
self.s.close(); time.sleep(0.3)
|
||||
self.s = open_racing(self.port)
|
||||
|
||||
def _wait_ack(self, what, tmo=3.0):
|
||||
self.s.timeout = tmo
|
||||
b = self.s.read(1)
|
||||
self.s.timeout = 1.0
|
||||
if b == bytes([ACK]):
|
||||
return True
|
||||
if b == bytes([NACK]):
|
||||
raise RuntimeError("%s: NACK" % what)
|
||||
raise RuntimeError("%s: no answer (%r)" % (what, b))
|
||||
|
||||
def _probe_get(self):
|
||||
"""Already synced? A GET (0x00) ACKs and returns the command list."""
|
||||
self.s.reset_input_buffer()
|
||||
self.s.write(bytes([0x00, 0xFF])); time.sleep(0.15)
|
||||
b = self.s.read(1)
|
||||
if b == bytes([ACK]):
|
||||
n = self.s.read(1)[0]; self.s.read(n + 1); self.s.read(1)
|
||||
return True
|
||||
return b == bytes([NACK])
|
||||
|
||||
def sync(self):
|
||||
# autobaud (works once per power-up)
|
||||
self.s.reset_input_buffer()
|
||||
self.s.write(b"\x7f"); time.sleep(0.15)
|
||||
b = self.s.read(1)
|
||||
if b in (bytes([ACK]), bytes([NACK])):
|
||||
return True
|
||||
# already synced this power cycle?
|
||||
if self._probe_get():
|
||||
return True
|
||||
raise RuntimeError("no bootloader sync (BOOT0 high? on USB power? replug it)")
|
||||
|
||||
def cmd(self, c):
|
||||
self.s.write(bytes([c, c ^ 0xFF]))
|
||||
return self._wait_ack("cmd 0x%02X" % c)
|
||||
|
||||
def get(self):
|
||||
self.cmd(0x00)
|
||||
n = self.s.read(1)[0]
|
||||
data = self.s.read(n + 1)
|
||||
self.s.read(1)
|
||||
return data
|
||||
|
||||
def readable(self):
|
||||
self.s.write(bytes([0x11, 0xEE]))
|
||||
b = self.s.read(1)
|
||||
if b == bytes([ACK]):
|
||||
# send a dummy address so the bootloader doesn't hang mid-command
|
||||
self._addr(FLASH); self.s.write(bytes([0, 0xFF])); self.s.read(1)
|
||||
self.s.read(1)
|
||||
return True
|
||||
return False
|
||||
|
||||
def _addr(self, addr):
|
||||
a = addr.to_bytes(4, "big")
|
||||
self.s.write(a + bytes([a[0] ^ a[1] ^ a[2] ^ a[3]]))
|
||||
self._wait_ack("address 0x%08X" % addr)
|
||||
|
||||
def unprotect(self):
|
||||
"""Readout-unprotect: mass-erase + drop RDP to 0. Erases factory firmware."""
|
||||
self.s.write(bytes([0x92, 0x6D]))
|
||||
self._wait_ack("readout-unprotect start")
|
||||
self._wait_ack("readout-unprotect erase", tmo=6.0)
|
||||
time.sleep(1.0) # chip resets; BOOT0 still high -> bootloader again
|
||||
self.reopen(); self.sync()
|
||||
|
||||
def mass_erase(self):
|
||||
self.s.write(bytes([0x44, 0xBB]))
|
||||
self._wait_ack("ext-erase start")
|
||||
self.s.write(bytes([0xFF, 0xFF, 0x00])) # 0xFFFF = global mass erase, cksum 0
|
||||
self._wait_ack("mass erase", tmo=8.0)
|
||||
|
||||
def write(self, addr, data):
|
||||
self.cmd(0x31)
|
||||
self._addr(addr)
|
||||
n = len(data) - 1
|
||||
cs = n
|
||||
for b in data:
|
||||
cs ^= b
|
||||
self.s.write(bytes([n]) + data + bytes([cs]))
|
||||
self._wait_ack("write @0x%08X" % addr, tmo=3.0)
|
||||
|
||||
def read(self, addr, n):
|
||||
self.cmd(0x11)
|
||||
self._addr(addr)
|
||||
self.s.write(bytes([n - 1, (n - 1) ^ 0xFF]))
|
||||
self._wait_ack("read @0x%08X" % addr)
|
||||
return self.s.read(n)
|
||||
|
||||
def go(self, addr):
|
||||
self.cmd(0x21)
|
||||
self._addr(addr)
|
||||
|
||||
|
||||
def main():
|
||||
args = [a for a in sys.argv[1:] if not a.startswith("--")]
|
||||
port = "COM16"
|
||||
if "--port" in sys.argv:
|
||||
port = sys.argv[sys.argv.index("--port") + 1]
|
||||
data = open(args[0], "rb").read()
|
||||
if len(data) % 4:
|
||||
data += b"\xFF" * (4 - len(data) % 4)
|
||||
print("firmware %s: %d bytes" % (args[0], len(data)))
|
||||
|
||||
bl = BL(port)
|
||||
bl.sync()
|
||||
time.sleep(0.1)
|
||||
bl.s.reset_input_buffer()
|
||||
print("sync ok")
|
||||
|
||||
# Safety gate: the ROM bootloader auto-baud can come back garbled after a
|
||||
# software jump. Prove the link is solid (several clean GETs) BEFORE erasing,
|
||||
# so a flaky link aborts without wiping a working firmware.
|
||||
good = 0
|
||||
for _ in range(8):
|
||||
try:
|
||||
d = bl.get()
|
||||
if len(d) >= 2 and d[1:].count(0x44) and d[1:].count(0x31):
|
||||
good += 1
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(0.02)
|
||||
if good < 8:
|
||||
print("link not reliable (%d/8 clean) - aborting BEFORE erase." % good)
|
||||
print("use the BOOT0 wire + a power-cycle for a clean ROM-bootloader link.")
|
||||
return 2
|
||||
print("link solid (%d/8)." % good)
|
||||
|
||||
# Erase. On an unlocked chip a mass-erase is enough; if it's still locked
|
||||
# (factory RDP1) the erase NACKs, so fall back to readout-unprotect.
|
||||
try:
|
||||
bl.mass_erase()
|
||||
print("mass-erased.")
|
||||
except RuntimeError:
|
||||
print("locked -> readout-unprotect (ERASES factory firmware)...")
|
||||
bl.unprotect()
|
||||
print("unlocked and erased.")
|
||||
|
||||
print("writing...")
|
||||
for off in range(0, len(data), 256):
|
||||
chunk = data[off:off + 256]
|
||||
bl.write(FLASH + off, chunk)
|
||||
print("verifying...")
|
||||
bad = 0
|
||||
for off in range(0, len(data), 256):
|
||||
chunk = data[off:off + 256]
|
||||
rb = bl.read(FLASH + off, len(chunk))
|
||||
if rb != chunk:
|
||||
bad += 1
|
||||
print(" mismatch at +0x%X" % off)
|
||||
if bad:
|
||||
print("VERIFY FAILED (%d blocks)" % bad); return 1
|
||||
print("verify OK. starting firmware...")
|
||||
bl.go(FLASH)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,151 @@
|
||||
"""Update the dongle application over USB via the permanent 'safeboot' stage 1.
|
||||
No BOOT0 wire, no ST-Link, no ROM bootloader. Writes only the app region
|
||||
(pages 4-15); safeboot itself is never touched.
|
||||
|
||||
usage: safeboot_flash.py app.bin [--port COM16] [--knock]
|
||||
|
||||
How it enters flash mode:
|
||||
1) if the app is running, 'b' makes it soft-reset into safeboot flash mode;
|
||||
2) otherwise (dead/absent app) replug the dongle and this spams 'S' to catch
|
||||
safeboot's cold-boot knock window.
|
||||
--knock skips 1) and keeps knocking for 2 minutes, reopening the port as it
|
||||
comes and goes: start it, then replug. Use it when the app doesn't answer.
|
||||
"""
|
||||
import sys, time, serial
|
||||
|
||||
APP_BASE = 0x08001000
|
||||
PORT = "COM16"
|
||||
|
||||
|
||||
def open_port(port, open_timeout=20.0):
|
||||
"""Retry the open to win the race against programs (SignalRGB) that grab
|
||||
CH340 COM ports on enumeration. Once held, the handle stays ours."""
|
||||
t = time.time()
|
||||
while True:
|
||||
try:
|
||||
return serial.Serial(port, 115200, timeout=0.3)
|
||||
except serial.SerialException:
|
||||
if time.time() - t > open_timeout:
|
||||
raise
|
||||
time.sleep(0.02)
|
||||
|
||||
|
||||
def enter_flash(s):
|
||||
# path 1: tell a running app to reset into safeboot
|
||||
s.reset_input_buffer()
|
||||
s.write(b"b")
|
||||
t = time.time()
|
||||
buf = b""
|
||||
while time.time() - t < 1.5:
|
||||
buf += s.read(64)
|
||||
if b"SBF" in buf:
|
||||
return True
|
||||
time.sleep(0.05)
|
||||
# path 2: knock (works during safeboot's cold-boot window, after a replug)
|
||||
t = time.time()
|
||||
while time.time() - t < 6.0:
|
||||
s.write(b"S")
|
||||
r = s.read(16)
|
||||
if b"K" in r or b"SBF" in r or b"SB1" in r:
|
||||
# drain, then confirm we're in flash mode
|
||||
time.sleep(0.1); s.reset_input_buffer()
|
||||
s.write(b"V"); time.sleep(0.1)
|
||||
if b"SBV" in s.read(16):
|
||||
return True
|
||||
time.sleep(0.1)
|
||||
return False
|
||||
|
||||
|
||||
def knock_until_caught(port, timeout=120.0):
|
||||
"""--knock: for an app that doesn't answer 'b' (dead or deaf). Keeps the
|
||||
port open and spams 'S' for up to `timeout` s, reopening whenever the port
|
||||
vanishes, so replugging the dongle at any moment lands in safeboot's 1.5 s
|
||||
cold-boot knock window. Returns the open port in flash mode, or None."""
|
||||
t = time.time()
|
||||
s = None
|
||||
while time.time() - t < timeout:
|
||||
try:
|
||||
if s is None:
|
||||
s = serial.Serial(port, 115200, timeout=0.02)
|
||||
s.write(b"S")
|
||||
r = s.read(16)
|
||||
if b"K" in r or b"SB1" in r or b"SBR" in r:
|
||||
time.sleep(0.1); s.reset_input_buffer(); s.timeout = 0.3
|
||||
s.write(b"V"); time.sleep(0.1)
|
||||
if b"SBV" in s.read(16):
|
||||
return s
|
||||
s.timeout = 0.02
|
||||
except (serial.SerialException, OSError):
|
||||
try:
|
||||
if s is not None:
|
||||
s.close()
|
||||
except Exception:
|
||||
pass
|
||||
s = None
|
||||
time.sleep(0.02)
|
||||
return None
|
||||
|
||||
|
||||
def expect(s, ch, what):
|
||||
r = s.read(1)
|
||||
if r != ch:
|
||||
raise RuntimeError("%s: expected %r got %r" % (what, ch, r))
|
||||
|
||||
|
||||
def main():
|
||||
args = [a for a in sys.argv[1:] if not a.startswith("--")]
|
||||
port = sys.argv[sys.argv.index("--port") + 1] if "--port" in sys.argv else PORT
|
||||
data = open(args[0], "rb").read()
|
||||
if len(data) % 2:
|
||||
data += b"\xFF"
|
||||
print("app %s: %d bytes -> 0x%08X" % (args[0], len(data), APP_BASE))
|
||||
|
||||
if "--knock" in sys.argv:
|
||||
print("knock mode: replug the dongle now (waiting up to 2 min)...")
|
||||
s = knock_until_caught(port)
|
||||
if s is None:
|
||||
print("no knock window caught")
|
||||
return 1
|
||||
else:
|
||||
s = open_port(port)
|
||||
print("entering safeboot flash mode...")
|
||||
if not enter_flash(s):
|
||||
print("could not enter flash mode (app not running? replug and retry with --knock)")
|
||||
return 1
|
||||
s.reset_input_buffer()
|
||||
s.write(b"V"); time.sleep(0.1)
|
||||
print("safeboot:", s.read(16).decode("ascii", "replace").strip())
|
||||
|
||||
print("erasing app region...")
|
||||
s.write(b"E"); s.timeout = 3.0; expect(s, b"K", "erase"); s.timeout = 0.5
|
||||
|
||||
print("writing...")
|
||||
for off in range(0, len(data), 128):
|
||||
chunk = data[off:off + 128]
|
||||
addr = APP_BASE + off
|
||||
s.write(b"W" + addr.to_bytes(4, "little") + bytes([len(chunk)]) + chunk)
|
||||
expect(s, b"K", "write @0x%08X" % addr)
|
||||
|
||||
print("verifying...")
|
||||
bad = 0
|
||||
for off in range(0, len(data), 128):
|
||||
chunk = data[off:off + 128]
|
||||
addr = APP_BASE + off
|
||||
s.write(b"R" + addr.to_bytes(4, "little") + bytes([len(chunk)]))
|
||||
rb = s.read(len(chunk))
|
||||
if rb != chunk:
|
||||
bad += 1
|
||||
print(" mismatch @0x%08X" % addr)
|
||||
if bad:
|
||||
print("VERIFY FAILED (%d blocks)" % bad); return 1
|
||||
|
||||
print("verify OK; booting app.")
|
||||
s.write(b"G"); expect(s, b"K", "go")
|
||||
time.sleep(0.3)
|
||||
print(s.read(256).decode("ascii", "replace").strip())
|
||||
s.close()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,37 @@
|
||||
# Archive: earlier designs
|
||||
|
||||
Designs that are no longer the current ones, kept for reference. The
|
||||
current remote is `remote_v2.scad` (see [../../README.md](../../README.md)).
|
||||
|
||||
## Original remote (`remote.scad`)
|
||||
|
||||

|
||||
|
||||
The remote used with the robot until 2026: a 208 x 175 x 45 mm box printed
|
||||
from the Fusion 360 design `../fusion360/remote/remote v40.f3d`.
|
||||
`remote.scad` rebuilds it from that file's solids (details, and the bugs
|
||||
fixed in the rebuild, in [PLANNING.md](PLANNING.md), section "Remote").
|
||||
|
||||
What's inside (from the owner and the photos below):
|
||||
|
||||
- Two joysticks, JH-D300X-R1 (3-axis twist grip), on the lid. The model
|
||||
uses the similar JH-D400X-R4 drawing.
|
||||
- Nextion NX4832T035 3.5" display under a window in the lid.
|
||||
- A Teensy 4.0 socketed on a cut perfboard with the nRF24L01 (100 uF cap)
|
||||
and an AMS1117 regulator.
|
||||
- An MPU6050 (GY-521) on the big standoff in the middle.
|
||||
- An 18650 UPS/boost board on two standoffs, with its charge and on LEDs
|
||||
visible through two small slots in the floor.
|
||||
- A USB-C breakout glued into the holder at the side port.
|
||||
- A slide switch mounted on top of the lid, and a lanyard loop on the bump.
|
||||
|
||||
Photos of the printed remote, opened: [original remote/](original%20remote/).
|
||||
|
||||
| | |
|
||||
|---|---|
|
||||
|  |  |
|
||||
|  |  |
|
||||
|
||||
Why it was replaced: the new remote (`remote_v2.scad`) is smaller
|
||||
(216 x 116 x 32 mm), prints in ASA without supports, hangs level from a neck
|
||||
strap (a balance-point eyelet and a body brace), and stands on a table.
|
||||
@@ -118,7 +118,7 @@ mm (sat in the ledge, posts overlapping the bosses); the display bezel's
|
||||
mesh facets became a clean window; the stray Body12 (an ON-OFF bracket
|
||||
line never cut in) is left out. The display is a Nextion NX4832T035
|
||||
(3.5" Basic); its board's M3 holes are the lid's 94.2 x 48.5 post grid.
|
||||
Reference parts in `remote.scad`: the joysticks are JH-D400X-R4 (4-axis,
|
||||
Reference parts in `remote.scad`: the joysticks are modelled as JH-D400X-R4 (owner's photos later showed JH-D300X-R1; 4-axis,
|
||||
X/Y ±25°, twist ±45°, button; owner), mounted with the dust cover on the
|
||||
lid's top face; their M2.5 square is 32.5 mm, so the lid's screw holes moved
|
||||
from the original's 33 mm square to 32.5. The battery is an 18650 UPS/boost
|
||||
@@ -129,14 +129,103 @@ Also from the owner: the bump on top of the middle post is a lanyard loop
|
||||
(the original's tunnel through it is modelled); the square hole is the on/off
|
||||
slide switch, mounted on top (same switch as the robot). Inside: an IMU on the
|
||||
21 x 9 mm block (assumed GY-521), a USB-C breakout on the side-port block, and
|
||||
a Teensy **4.0** (confirmed by the owner, although `platformio.ini` builds
|
||||
the remote for a 4.1); it has no mounting holes and is placed on the
|
||||
a Teensy **4.0** (confirmed by the owner; `platformio.ini` now builds
|
||||
the remote for it); it has no mounting holes and is placed on the
|
||||
hole-less 8 x 24 mm block as a guess.
|
||||
Open: what the 6 mm x 10 mm holes in the bottom bosses hold (magnets?), and
|
||||
whether the upper two bosses are meant to be asymmetric (x -80.387 / +82.73).
|
||||
|
||||
## Remote v2 (`remote_v2.scad`), draft 4
|
||||
|
||||
A new remote for the same electronics; `remote.scad` stays as the record of
|
||||
the printed one. How it's used (owner): the sticks are big twist grips that
|
||||
take a whole hand each, so nothing holds the case. It hangs from a neck strap
|
||||
or lies on a table. Earlier drafts: gamepad grips (dropped: nobody holds
|
||||
them), strap tabs at the rear corners (it tipped forward), a hexapod emblem
|
||||
inlay (owner preferred a honeycomb).
|
||||
|
||||
From the owner's photos and drawing: the sticks are JH-D300X-R1; the Teensy
|
||||
sits socketed on a cut perfboard (56 x 80, measured) with the nRF24L01
|
||||
(100 uF cap), an AMS1117, the ribbon connectors and now also the GY-521; the
|
||||
switch's plate sits on top of the case, body through the slot, and the old
|
||||
8 x 16 slot with d2 holes 19 mm apart fits it exactly; the UPS board's
|
||||
charge/on LEDs show through two slots in the floor.
|
||||
|
||||
- Size 216 x 116 x 42 mm (top shell 10 mm taller than first printed, TOP_GROW: room behind the display) (old remote: 208 x 175 x 45). Sticks at u +-80,
|
||||
display forward of them, strap eyelet behind the display, UPS board behind
|
||||
that, switch at the rear centre. Main board under the display.
|
||||
- JH-D300X-R1: dust cover d53.5 on the top face; the 40.4 mm control box
|
||||
(25.5 tall) against the plate's underside, 4 x M2.5 x 14 through cover and
|
||||
plate into the box (no nuts). Reaches 28.5 mm down. Pots (d16.5, 9.2 out)
|
||||
towards the middle and towards the user.
|
||||
- Display mounted turned 180 deg so its J1 connector is on the right, clear
|
||||
of the Teensy's USB plug. `remote/remote.HMI` is generated for that
|
||||
(direction 270 in the Nextion Editor's terms; done 2026-11-17).
|
||||
Glass 1.0 mm under the top face, board on 4 posts tapped M3 (3.66 mm of thread, 1.2 mm under the top face);
|
||||
LCD 85.7 x 55 x 3.86 plus a 1.3 mm flex cable on one side, 11.21 mm glass to
|
||||
back of J1 (owner measured).
|
||||
- Main board clamped at 4 points on its long sides, no holes in the board
|
||||
(owner): posts beside the edges carry it, M3 x 8 button heads with d7
|
||||
washers in heat-set inserts clamp the edge. 2 mm above the kickstand
|
||||
recess for the wires underneath; Teensy micro-USB to the left, nRF24 to
|
||||
the right. The cable plug clears the display board by ~0.2 mm and the left
|
||||
stick's pot narrowly: plug and board sizes are estimates.
|
||||
- UPS board on 2 standoffs behind the eyelet, Type-C end to the right (room
|
||||
for the cut cable's plug); LEDs through 2 x 6 mm floor slots.
|
||||
- Strap eyelet (separate part, 2 x M3 into heat-set inserts): on the top face
|
||||
behind the display (slots +-5 mm to balance it), or on the front face (the
|
||||
edge away from you, 2 more inserts at Z -19). Folded forward, the bail
|
||||
hits a front-mounted eyelet: then park the bail back or down.
|
||||
- Brace bail: U pivoting on the side faces (v -25, Z -22), M3 x 16 into
|
||||
heat-set inserts; 0 = folded forward, -150 = brace against chest or belly,
|
||||
-180 = straight back. Swings down under the case.
|
||||
- Kickstand: 30 x 35 mm flap in a recess in the underside, hinged at the front
|
||||
on an M3 x 40 into a nut trap; lifts the front a little on a table.
|
||||
- Shells split at Z -12, both print without supports (top face down, bottom
|
||||
on its flat underside), 45 deg chamfers, 1.2 mm locating lip, 4 x M3 x 12
|
||||
from below into heat-set inserts. USB-C breakout glued into a pocket at
|
||||
the front left, receptacle 0.6 mm inside the front face.
|
||||
- Inlay (second colour, first 3 layers of the face-down top shell): ON/OFF
|
||||
plus a honeycomb of whole cells (4.5 mm, 1.2 mm walls) right of a diagonal
|
||||
through the middle, clear of display, sticks, eyelet and switch; lone
|
||||
cells dropped. Colours (owner, ASA): black case, red honeycomb (PART
|
||||
"inlay"), white ON/OFF (PART "text"); load "top", "inlay" and "text" as one
|
||||
multi-part object. Each extra colour adds a swap on the first 3 layers only.
|
||||
- Printability (the STLs sliced in 0.2 mm layers, every area not carried by
|
||||
the layer below listed): the brace pivot bosses stand on pillars and the
|
||||
lip's tie band steps out at 45 deg (both were floating overhangs in the
|
||||
first export); what's left are bridges: the 30 mm kickstand-recess roof,
|
||||
the hinge trough, hole and counterbore tops, the USB-C opening.
|
||||
- Checked: no part collides with the shells or with each other, the shells
|
||||
and the inlay don't overlap, kickstand clear at 0 and 70 deg, bail clear
|
||||
from 0 to -180 deg (CGAL intersections).
|
||||
- Open: component positions on the main board, Teensy cable plug size, LCD thickness, UPS
|
||||
hole spacing; hand room untested; set `IMU_AXIS_FROM_CHIP` /
|
||||
`IMU_AXIS_SIGN` in `remote/Config.h` for the IMU on the main board.
|
||||
|
||||
## Notes for future designs
|
||||
|
||||
- Robot power (still to choose): a battery or supply that gives a **stable
|
||||
6 V** with enough current for all 18 servos (12 x HS-485HB + 6 stronger
|
||||
ones of the same size). Servos draw well over 1 A each when stalled or
|
||||
starting, and many move at once while walking, so plan for a 6 V regulator
|
||||
/ UBEC of 15-20 A peak (e.g. a 2S LiPo through a high-current 6 V BEC), or
|
||||
a pack that stays near 6 V under that load. Check the servos' datasheet
|
||||
for the stall current. The battery must also fit the bay (see open
|
||||
question 3) and leave room for the servo plugs.
|
||||
|
||||
- Remote: the IMU is on the normal pin headers (done 2026-11-17: Wire on
|
||||
18/19, the two stick axes that were there moved to A7/A8; display on
|
||||
Serial2, 7/8). Pinout in `remote/README.md`, "Wiring".
|
||||
|
||||
- Remote: measure the 18650's voltage on the Teensy to show how much charge
|
||||
is left (owner, later). Divider ~100k/100k from the cell's + (holder or the
|
||||
UPS board's B+, not the 5 V output) to A6 (pin 20, free), 100 nF to GND;
|
||||
full cell = ~2.1 V at the pin. Firmware: reading, smoothing and the volts
|
||||
on the STATUS page are done (`remote/Battery`, pin still 255 = not wired);
|
||||
still to do: Li-ion curve to %, and a low-battery warning. The robot
|
||||
already sends its own battery voltage back in the telemetry.
|
||||
|
||||
- Servo wires barely fit on the real robot: the six servo headers hang
|
||||
above the battery bay, and the front pair sits right over the bay's end
|
||||
stop (X 28.8..31.8, top at Z 13.5). Kept as-is for now; give the plugs and
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
"""Combine STL parts into one multi-part 3MF for OrcaSlicer / PrusaSlicer.
|
||||
|
||||
python export_3mf.py (from hardware/openscad, after the STL export)
|
||||
|
||||
Writes stl/remote_v2/remote_v2_top_inlays.3mf: the top shell with its
|
||||
two inlays as ONE object with three parts, in the same frame, each part on
|
||||
its own extruder/filament (1 = case, 2 = honeycomb, 3 = ON/OFF text). The
|
||||
part list is in PARTS below. Uses the PrusaSlicer 3MF layout (one mesh, the
|
||||
parts as triangle ranges in Metadata/Slic3r_PE_model.config), which
|
||||
OrcaSlicer and PrusaSlicer both read.
|
||||
"""
|
||||
import os
|
||||
import zipfile
|
||||
|
||||
HERE = os.path.dirname(os.path.abspath(__file__))
|
||||
OUT_DIR = os.path.join(HERE, "stl", "remote_v2")
|
||||
|
||||
# (file, part name, extruder)
|
||||
PARTS = [
|
||||
("remote_v2_top.stl", "top shell (black)", 1),
|
||||
("remote_v2_inlay.stl", "honeycomb inlay (red)", 2),
|
||||
("remote_v2_text.stl", "ON-OFF inlay (white)", 3),
|
||||
]
|
||||
OUT = "remote_v2_top_inlays.3mf" # remote_v2_top_multicolor.3mf is the owner's OrcaSlicer project
|
||||
|
||||
|
||||
def read_stl(path):
|
||||
"""Triangles of an ASCII or binary STL as lists of 3 vertex tuples."""
|
||||
data = open(path, "rb").read()
|
||||
tris = []
|
||||
if data[:5] == b"solid" and b"facet" in data[:400]:
|
||||
verts = []
|
||||
for line in data.decode("ascii", "replace").splitlines():
|
||||
t = line.split()
|
||||
if t and t[0] == "vertex":
|
||||
verts.append(tuple(float(x) for x in t[1:4]))
|
||||
if len(verts) == 3:
|
||||
tris.append(verts)
|
||||
verts = []
|
||||
else:
|
||||
import struct
|
||||
n = struct.unpack_from("<I", data, 80)[0]
|
||||
for i in range(n):
|
||||
f = struct.unpack_from("<12f", data, 84 + 50 * i)
|
||||
tris.append([tuple(f[3:6]), tuple(f[6:9]), tuple(f[9:12])])
|
||||
return tris
|
||||
|
||||
|
||||
def main():
|
||||
vertices, index, triangles, ranges = [], {}, [], []
|
||||
for fname, name, extruder in PARTS:
|
||||
first = len(triangles)
|
||||
for tri in read_stl(os.path.join(OUT_DIR, fname)):
|
||||
ids = []
|
||||
for v in tri:
|
||||
key = (round(v[0], 5), round(v[1], 5), round(v[2], 5), len(ranges)) # no sharing between parts
|
||||
if key not in index:
|
||||
index[key] = len(vertices)
|
||||
vertices.append(v)
|
||||
ids.append(index[key])
|
||||
if len(set(ids)) == 3:
|
||||
triangles.append(ids)
|
||||
ranges.append((first, len(triangles) - 1, name, extruder))
|
||||
|
||||
model = ['<?xml version="1.0" encoding="UTF-8"?>',
|
||||
'<model unit="millimeter" xml:lang="en-US" '
|
||||
'xmlns="http://schemas.microsoft.com/3dmanufacturing/core/2015/02" '
|
||||
'xmlns:slic3rpe="http://schemas.slic3r.org/3mf/2017/06">',
|
||||
' <metadata name="slic3rpe:Version3mf">1</metadata>',
|
||||
' <metadata name="Title">remote_v2 top shell with inlays</metadata>',
|
||||
' <resources>',
|
||||
' <object id="1" type="model">',
|
||||
' <mesh>',
|
||||
' <vertices>']
|
||||
model += [' <vertex x="%.5f" y="%.5f" z="%.5f"/>' % v for v in vertices]
|
||||
model += [' </vertices>', ' <triangles>']
|
||||
model += [' <triangle v1="%d" v2="%d" v3="%d"/>' % tuple(t) for t in triangles]
|
||||
model += [' </triangles>', ' </mesh>', ' </object>', ' </resources>',
|
||||
' <build>', ' <item objectid="1"/>', ' </build>', '</model>']
|
||||
|
||||
config = ['<?xml version="1.0" encoding="UTF-8"?>', '<config>',
|
||||
' <object id="1" instances_count="1">',
|
||||
' <metadata type="object" key="name" value="remote_v2_top_multicolor"/>']
|
||||
for first, last, name, extruder in ranges:
|
||||
config += [' <volume firstid="%d" lastid="%d">' % (first, last),
|
||||
' <metadata type="volume" key="name" value="%s"/>' % name,
|
||||
' <metadata type="volume" key="volume_type" value="ModelPart"/>',
|
||||
' <metadata type="volume" key="extruder" value="%d"/>' % extruder,
|
||||
' </volume>']
|
||||
config += [' </object>', '</config>']
|
||||
|
||||
content_types = ('<?xml version="1.0" encoding="UTF-8"?>\n'
|
||||
'<Types xmlns="http://schemas.openxmlformats.org/package/2006/content-types">\n'
|
||||
' <Default Extension="rels" ContentType="application/vnd.openxmlformats-package.relationships+xml"/>\n'
|
||||
' <Default Extension="model" ContentType="application/vnd.ms-package.3dmanufacturing-3dmodel+xml"/>\n'
|
||||
' <Default Extension="config" ContentType="text/xml"/>\n'
|
||||
'</Types>\n')
|
||||
rels = ('<?xml version="1.0" encoding="UTF-8"?>\n'
|
||||
'<Relationships xmlns="http://schemas.openxmlformats.org/package/2006/relationships">\n'
|
||||
' <Relationship Target="/3D/3dmodel.model" Id="rel0" '
|
||||
'Type="http://schemas.microsoft.com/3dmanufacturing/2013/01/3dmodel"/>\n'
|
||||
'</Relationships>\n')
|
||||
|
||||
path = os.path.join(OUT_DIR, OUT)
|
||||
with zipfile.ZipFile(path, "w", zipfile.ZIP_DEFLATED) as z:
|
||||
z.writestr("[Content_Types].xml", content_types)
|
||||
z.writestr("_rels/.rels", rels)
|
||||
z.writestr("3D/3dmodel.model", "\n".join(model) + "\n")
|
||||
z.writestr("Metadata/Slic3r_PE_model.config", "\n".join(config) + "\n")
|
||||
print("%s: %d vertices, %d triangles, parts %s" % (OUT, len(vertices), len(triangles),
|
||||
", ".join("%s %d-%d" % (r[2], r[0], r[1]) for r in ranges)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -150,8 +150,9 @@ module nextion_3_5() {
|
||||
color("#3050a0") translate([win[0] - 73.44/2, win[1] - 48.96/2, DISPLAY_POCKET_Z - 0.2]) cube([73.44, 48.96, 0.2]);
|
||||
}
|
||||
|
||||
// JH-D400X-R4 4-axis joystick (X/Y +-25 deg, twist +-45 deg, button),
|
||||
// from its dimension drawing. Mounted with its dust cover on the lid's top
|
||||
// Joystick, modelled from the JH-D400X-R4 drawing (4-axis, X/Y +-25 deg,
|
||||
// twist +-45 deg, button). The remote's sticks turned out to be JH-D300X-R1
|
||||
// (label on the box, owner's photo): similar, dimensions not checked yet. Mounted with its dust cover on the lid's top
|
||||
// face; the body hangs through the d40 opening. Local frame: stick axis on
|
||||
// Z, Z = 0 on the dust cover's underside. Pots on +X and +Y.
|
||||
JOY_COVER_D = 53.5;
|
||||
|
||||
@@ -0,0 +1,588 @@
|
||||
// Remote v2: a flat console for the same electronics as the original remote
|
||||
// (remote.scad, kept as the record of the printed one). Draft 3, not printed.
|
||||
//
|
||||
// Use (owner): both hands wrap the big twist-grip sticks, so nothing holds
|
||||
// the case. It hangs from a neck strap on one eyelet on the top face, near
|
||||
// the balance point and behind the display (slide it in its slots to balance),
|
||||
// with a fold-out bail that braces against the body; or it lies on a table on
|
||||
// its kickstand. The sticks sit far apart and nothing rises above the top face
|
||||
// near them.
|
||||
//
|
||||
// Two shells split horizontally, printed in ASA: the top shell face down,
|
||||
// the bottom shell on its flat underside, both without supports. The
|
||||
// kickstand, strap eyelet and brace bail are separate parts. The shells are joined by M3 screws from below
|
||||
// into heat-set inserts in the top shell.
|
||||
//
|
||||
// Frame: u to the right, v away from the user (the far edge is the front),
|
||||
// Z up, Z = 0 on the top face.
|
||||
//
|
||||
// Electronics (all reused): 2 x JH-D300X-R1 joystick, Nextion NX4832T035
|
||||
// (3.5"), 18650 UPS/boost board (89 x 22), Teensy 4.0, GY-521 (MPU6050),
|
||||
// 12 x 15 mm 4-pin USB-C breakout (V, D-, D+, G), slide switch (the robot's).
|
||||
// Wiring (owner): the breakout's V/G go to the UPS board's charge input and
|
||||
// D-/D+ to the Teensy, through two halves of a cut USB cable whose plugs go
|
||||
// into the UPS board and the Teensy; the UPS output feeds the Teensy.
|
||||
|
||||
|
||||
/* [Shell] */
|
||||
T = 2.4; // walls and floor (6 perimeters of 0.4)
|
||||
PLATE = 3.0; // top plate
|
||||
TOP_GROW = 10; // extra height of the top shell only (room behind the display); the bottom shell is unchanged
|
||||
Z_BOT = -32 - TOP_GROW; // flat underside (32 mm thick before TOP_GROW: the D300X sticks reach 28.5 down)
|
||||
Z_SPLIT = -12 - TOP_GROW; // top shell / bottom shell
|
||||
CHAMFER = 4; // 45 deg bottom edge (printable without support)
|
||||
TOP_CHAMFER = 1.5; // 45 deg top edge (the top shell prints face down)
|
||||
LIP_H = 2.5; // locating lip on the bottom shell, into the top shell
|
||||
LIP_W = 1.2;
|
||||
FIT = 0.2; // clearance per side between printed parts
|
||||
|
||||
/* [Outline] */
|
||||
STICKS = [[-80, 0], [80, 0]];
|
||||
STICK_SQ = [28, 40]; // half size (u, v) of the rounded box around each stick
|
||||
STICK_SQ_R = 10;
|
||||
FRONT_BAND = [[-60, 0], [60, 52]]; // u, v box: display, USB-C, Teensy
|
||||
FRONT_V = FRONT_BAND[1][1];
|
||||
SIDE_U = STICKS[1][0] + STICK_SQ[0]; // the flat side faces
|
||||
REAR_BAND = [[-80, -64], [80, 0]]; // UPS board behind the strap eyelet, switch behind that
|
||||
CORNER_R = 10;
|
||||
|
||||
/* [Fasteners] */
|
||||
// M3 x 12 from below; heat-set insert M3 (4.0 mm hole, 5.7 mm long) in the top shell
|
||||
SCREWS = [[-58, 32], [58, 32], [-70, -52], [70, -52]];
|
||||
BOSS_D = 9;
|
||||
INSERT_D = 4.0;
|
||||
INSERT_L = 6;
|
||||
SCREW_D = 3.4;
|
||||
HEAD_D = 6.5;
|
||||
HEAD_Z = Z_SPLIT - 7; // counterbore floor: 7 mm of tower + 5 mm into the insert
|
||||
|
||||
/* [Joysticks] */
|
||||
STICK_HOLE_D = 40;
|
||||
STICK_SCREW = 16.25; // 4 x M2.5 x 14 on a 32.5 mm square, through cover and plate into the control box
|
||||
STICK_SCREW_D = 2.8;
|
||||
|
||||
/* [Display: Nextion NX4832T035] */
|
||||
NX_BOARD = [100.6, 55]; // measured by the owner
|
||||
NX_HOLES = [94.2, 48.5]; // M3 grid, centred on the board
|
||||
NX_CENTRE = [0, 20]; // board centre (forward of the sticks, so the strap misses it)
|
||||
NX_FLIP = true; // display mounted turned 180 deg (its J1 connector on the right, clear of
|
||||
// the Teensy's USB plug): set display direction 180 in the Nextion Editor
|
||||
NX_WIN_OFF = [NX_FLIP ? 2.15 : -2.15, 0]; // window (visible area) centre from the board centre
|
||||
NX_WINDOW = [75.9, 50.9]; // opening in the plate
|
||||
NX_LCD = [85.7, 55]; // LCD module outline (owner: 7.45 mm of board beside it on both sides, centred on the board)
|
||||
NX_LCD_CABLE = 1.3; // its flex cable sticks out this much on one short side (87 mm overall)
|
||||
NX_LCD_T = 3.86; // LCD thickness (owner)
|
||||
NX_BACK = 11.21 - NX_LCD_T - 1.6; // board underside to back of the J1 connector (owner: 11.21 from the glass)
|
||||
NX_BEZEL = 1.0; // plate left above the glass (was 1.5: the display sat on the Teensy)
|
||||
NX_POST_D = 5.5; // clears the LCD module (with its cable) by 0.2 mm
|
||||
NX_SCREW_D = 2.5; // M3 tap drill: tap the posts for M3 x 5 machine screws through the board
|
||||
NX_SKIN = 1.2; // plastic left between the tapped hole and the top face
|
||||
|
||||
/* [Switch: slide switch, mounted on top] */
|
||||
SWITCH_POS = [0, -56];
|
||||
SWITCH_SLOT = [16, 8]; // 8 x 16 slot and d2 holes 19 apart: fit the switch exactly (owner, old remote)
|
||||
SWITCH_PITCH = 19;
|
||||
SWITCH_HOLE_D = 2.0;
|
||||
ENGRAVE_D = 0.6; // ON/OFF inlay: the first 3 layers (0.2) of the face-down top shell
|
||||
|
||||
/* [USB-C breakout] */
|
||||
USBC_BOARD = [12, 15, 1.6];
|
||||
USBC_RCPT = [8.94, 7.35, 3.26]; // receptacle, flush with the board's front edge
|
||||
USBC_POS = [-45, 0]; // board centre u (v follows from the front wall)
|
||||
USBC_Z = Z_BOT + 10; // board underside
|
||||
|
||||
/* [Other boards] */
|
||||
UPS_CENTRE = [0, -41];
|
||||
UPS_LEDS = [[-23.9, 0], [23.9, 0]]; // charge / on LEDs, seen through 2 x 6 mm floor slots (as on the old remote)
|
||||
UPS_HOLES = 27.9; // +- u, not confirmed (same as the old remote)
|
||||
UPS_Z = Z_BOT + T + 2; // board underside (2 mm standoffs); its top ends 2 mm under the plate
|
||||
|
||||
/* [Main board: perfboard with the Teensy 4.0 (socketed), nRF24L01, AMS1117] */
|
||||
// Cut perfboard, 56 x 80 mm (owner). Component positions are estimates from the
|
||||
// owner's photos. Board frame: x across (0..56), y
|
||||
// along (0..80), Teensy micro-USB at y 0, nRF24 overhanging at y 80.
|
||||
PERF = [56, 80, 1.6]; // measured by the owner: 56 x 80
|
||||
// Held by 4 clamps on its long sides, no holes in the board (owner): a post
|
||||
// beside the edge carries the board's edge on its inner half, and an M3 x 8
|
||||
// button head with a washer (d7) in a heat-set insert in the post clamps it.
|
||||
PERF_CLAMPS = [[0, 4], [0, 68], [PERF[0], 4], [PERF[0], 68]]; // board frame: on the long edges x = 0 / PERF[0], at these y
|
||||
PERF_CLAMP_OUT = 1.5; // post centre outside the board edge (washer overlaps the board ~2 mm)
|
||||
PERF_POS = [-36, 37]; // where the board's (0, 0) corner goes; x runs to -v, y to +u
|
||||
PERF_Z = Z_BOT + 7.5; // board underside: 2 mm over the kickstand recess roof for wires (the Teensy USB plug then clears the display board)
|
||||
PERF_IMU = [23, 38]; // GY-521 on the main board (owner), on 8.5 mm headers: its corner, board frame
|
||||
|
||||
/* [Kickstand] */
|
||||
KS_W = 30; // flap width (u)
|
||||
KS_V = [8, 43.75]; // free end, hinge axis (the trough clears the main board)
|
||||
KS_T = 3;
|
||||
KS_BARREL_D = 6.5;
|
||||
KS_PIN_D = 3.3; // M3 x 40 screw, from inside on the left, into a nut trap on the right
|
||||
KS_WALL = 3; // side walls (nut trap)
|
||||
KS_WALL_V = 2; // front/back walls
|
||||
|
||||
/* [Strap eyelet] */
|
||||
// A loop on the top face for the neck strap's snap hook, near the balance
|
||||
// point (estimated centre of mass v -3, Z -18: with the eyelet at v -24 the
|
||||
// remote hangs level when the strap leaves at ~34 deg from vertical).
|
||||
// Two M3 x 8 button heads into heat-set inserts; the slots give
|
||||
// +-HOOK_TRAVEL to balance it.
|
||||
HOOK_POS = [0, -24];
|
||||
HOOK_TRAVEL = 5;
|
||||
HOOK_INSERTS = 7; // +- u
|
||||
HOOK_BASE = [24, 18, 4]; // u, v, Z
|
||||
// Alternative place: the front face (the edge away from you), 2 inserts in
|
||||
// the front wall of the bottom shell; the loop then points forward.
|
||||
HOOK_FRONT_Z = Z_SPLIT - 7;
|
||||
HOOK_MOUNT = "top"; // [top, front] (assembly view only)
|
||||
HOOK_HOLE_D = 8; // for the snap hook
|
||||
HOOK_H = 16; // top of the loop above the top face
|
||||
|
||||
/* [Brace bail] */
|
||||
// A U-shaped bail pivoting on the side faces, outside the case and below the
|
||||
// hands. Folded forward it wraps the front edge; swung back and down its
|
||||
// crossbar rests against your chest or belly. M3 x 16 button-head screws
|
||||
// with a washer into heat-set inserts in the side walls; friction holds it.
|
||||
BAIL_PIVOT = [-25, Z_BOT + 10]; // v, Z on the side faces (u +-SIDE_U)
|
||||
BAIL_L = 82; // pivot to crossbar centre line
|
||||
BAIL_BOW = 10; // crossbar bows outwards by this much in the middle
|
||||
BAIL_T = 5; // arm thickness (u)
|
||||
BAIL_H = 12; // arm and crossbar height
|
||||
BAIL_GAP = 0.5; // arm to side face (a washer fills it)
|
||||
BAIL_ANGLE = 0; // 0 = folded forward, swings down: -90 hanging, -180 straight back, ~-150 brace
|
||||
|
||||
/* [Rendering] */
|
||||
PART = "assembly"; // [assembly, top, inlay, text, bottom, kickstand, eyelet, bail]
|
||||
SHOW_TOP = true;
|
||||
SHOW_BOTTOM = true;
|
||||
SHOW_PARTS = true;
|
||||
SHOW_STICKS = true;
|
||||
KS_OPEN = 0; // kickstand angle, deg
|
||||
EXPLODE = 0;
|
||||
|
||||
/* [Hidden] */
|
||||
$fa = 4; $fs = 0.5;
|
||||
BIG = 400;
|
||||
JOIN = 1;
|
||||
|
||||
module lx(h) linear_extrude(h, convexity = 10) children();
|
||||
module rbox(b, r) translate(b[0] + [r, r]) offset(r = r) square(b[1] - b[0] - [2 * r, 2 * r]);
|
||||
module rsq(c, h, r) translate(c) offset(r = r) square(2 * (h - [r, r]), center = true);
|
||||
|
||||
// ---------- outline and solid ----------------------------------------------
|
||||
|
||||
module outline_2d() hull() {
|
||||
rbox(FRONT_BAND, CORNER_R);
|
||||
rbox(REAR_BAND, CORNER_R);
|
||||
for (s = STICKS) rsq(s, STICK_SQ, STICK_SQ_R);
|
||||
}
|
||||
|
||||
// The outline swept with a 45 deg chamfer at the bottom (and optionally at
|
||||
// the top): a thin slab of the outline, inset by the chamfer, Minkowski'd
|
||||
// with a turned profile. `inset` gives the inner surface of the walls.
|
||||
module swept(inset, z0, z1, top_chamfer) {
|
||||
c = CHAMFER;
|
||||
h = z1 - z0;
|
||||
tc = top_chamfer;
|
||||
minkowski() {
|
||||
translate([0, 0, z0]) linear_extrude(0.01) offset(delta = -(c + inset)) outline_2d();
|
||||
rotate_extrude($fn = 32) polygon([[0, 0], [c, c], [c, h - tc], [c - tc, h], [0, h]]);
|
||||
}
|
||||
}
|
||||
|
||||
module outer_solid() swept(0, Z_BOT, 0, TOP_CHAMFER);
|
||||
module inner_solid() swept(T, Z_BOT + T, BIG / 4, 0);
|
||||
|
||||
// ---------- top shell -------------------------------------------------------
|
||||
|
||||
module nx_holes() for (du = [-1, 1], dv = [-1, 1]) translate(NX_CENTRE + [du * NX_HOLES[0], dv * NX_HOLES[1]] / 2) children();
|
||||
module stick_screws() for (s = STICKS, du = [-1, 1], dv = [-1, 1]) translate(s + [du, dv] * STICK_SCREW) children();
|
||||
NX_WIN = NX_CENTRE + NX_WIN_OFF;
|
||||
NX_BOARD_TOP = -NX_BEZEL - NX_LCD_T; // board's top face
|
||||
|
||||
module top_shell() render(convexity = 10) difference() {
|
||||
union() {
|
||||
intersection() {
|
||||
outer_solid();
|
||||
difference() {
|
||||
translate([-BIG / 2, -BIG / 2, Z_SPLIT]) cube([BIG, BIG, -Z_SPLIT]);
|
||||
difference() { inner_solid(); translate([-BIG / 2, -BIG / 2, -PLATE]) cube(BIG); }
|
||||
}
|
||||
}
|
||||
// eyelet inserts
|
||||
for (m = [-1, 1]) translate([HOOK_POS[0] + m * HOOK_INSERTS, HOOK_POS[1], -PLATE - 7 + JOIN]) cylinder(d = 8, h = 7);
|
||||
// screw bosses
|
||||
for (s = SCREWS) translate([s[0], s[1], Z_SPLIT]) cylinder(d = BOSS_D, h = -Z_SPLIT - PLATE + JOIN);
|
||||
// display posts
|
||||
nx_holes() translate([0, 0, NX_BOARD_TOP]) cylinder(d = NX_POST_D, h = -NX_BOARD_TOP - PLATE + JOIN);
|
||||
// joystick nut bosses
|
||||
}
|
||||
// display: window (chamfered to the top) and LCD pocket
|
||||
translate(concat(NX_WIN, -NX_BEZEL - 0.01)) hull() {
|
||||
translate([0, 0, 0.005]) cube(concat(NX_WINDOW, 0.01), center = true);
|
||||
translate([0, 0, NX_BEZEL + 0.01]) cube(concat(NX_WINDOW + [2, 2] * NX_BEZEL, 0.02), center = true);
|
||||
}
|
||||
// LCD pocket: cable allowance on both short sides, so it fits either way round
|
||||
translate(concat(NX_CENTRE - (NX_LCD + [2 * NX_LCD_CABLE + 0.4, 1]) / 2, -PLATE - 1)) cube(concat(NX_LCD + [2 * NX_LCD_CABLE + 0.4, 1], PLATE + 1 - NX_BEZEL));
|
||||
nx_holes() translate([0, 0, NX_BOARD_TOP - 1]) cylinder(d = NX_SCREW_D, h = 1 - NX_BOARD_TOP - NX_SKIN);
|
||||
// joysticks
|
||||
for (s = STICKS) translate([s[0], s[1], -BIG / 2]) cylinder(d = STICK_HOLE_D, h = BIG);
|
||||
stick_screws() translate([0, 0, -BIG / 2]) cylinder(d = STICK_SCREW_D, h = BIG);
|
||||
// switch
|
||||
translate(SWITCH_POS) {
|
||||
translate([-SWITCH_SLOT[0] / 2, -SWITCH_SLOT[1] / 2, -BIG / 2]) cube([SWITCH_SLOT[0], SWITCH_SLOT[1], BIG]);
|
||||
for (m = [-1, 1]) translate([m * SWITCH_PITCH / 2, 0, -BIG / 2]) cylinder(d = SWITCH_HOLE_D, h = BIG);
|
||||
}
|
||||
// inserts
|
||||
for (s = SCREWS) translate([s[0], s[1], Z_SPLIT - 1]) cylinder(d = INSERT_D, h = 1 + INSERT_L);
|
||||
translate([0, 0, -ENGRAVE_D]) lx(1 + ENGRAVE_D) top_inlay_2d(); // ON/OFF and honeycomb inlays
|
||||
for (m = [-1, 1]) translate([HOOK_POS[0] + m * HOOK_INSERTS, HOOK_POS[1], -PLATE - 7]) {
|
||||
cylinder(d = INSERT_D, h = 7 + PLATE - 1);
|
||||
cylinder(d = SCREW_D, h = BIG);
|
||||
}
|
||||
}
|
||||
|
||||
// ON/OFF text, cut into the top face and printed as a second-colour inlay
|
||||
// (PART = "inlay", exported in the same frame as PART = "top").
|
||||
module switch_text_2d() for (t = [["ON", -1], ["OFF", 1]]) translate([t[1] * 19, 0])
|
||||
text(t[0], size = 5, halign = "center", valign = "center", font = "Liberation Sans:style=Bold");
|
||||
// Honeycomb on the right of a diagonal through the middle of the top face.
|
||||
// Only whole cells are drawn (every wall full width): a cell is kept when its
|
||||
// outer edge lies right of the diagonal, inside the top face (EDGE_KEEP from
|
||||
// the outline) and clear of the display window, the stick covers, the strap
|
||||
// eyelet (with its travel) and the switch with its ON/OFF text.
|
||||
HEX_R = 4.5; // cell circumradius (wall centre lines)
|
||||
HEX_LINE = 1.2; // wall width (3 extrusion widths)
|
||||
DIAG_POINT = [0, -14]; // the diagonal passes through here...
|
||||
DIAG_ANGLE = 20; // ...leaning this far from the v axis (top end to the right)
|
||||
EDGE_KEEP = 3; // from the outline (clear of the top chamfer)
|
||||
HEX_GAP = 1.5; // around the keep-outs
|
||||
|
||||
HEX_OUT = (HEX_R + HEX_LINE / 2 / cos(30)); // outer circumradius of a cell
|
||||
// The outline is the hull of rounded boxes with one corner radius, so it is
|
||||
// this convex polygon grown by CORNER_R (corners listed counter-clockwise).
|
||||
function _ol_pts() = let(r = CORNER_R, f = FRONT_BAND, b = REAR_BAND,
|
||||
su = STICKS[1][0] + STICK_SQ[0] - r, sv = STICK_SQ[1] - r)
|
||||
[[f[0][0] + r, f[1][1] - r], [-su, sv], [-su, -sv], [b[0][0] + r, b[0][1] + r],
|
||||
[b[1][0] - r, b[0][1] + r], [su, -sv], [su, sv], [f[1][0] - r, f[1][1] - r]];
|
||||
function _cross(a, b, q) = (b[0] - a[0]) * (q[1] - a[1]) - (b[1] - a[1]) * (q[0] - a[0]);
|
||||
function _seg_dist(a, b, q) = let(d = b - a, t = max(0, min(1, ((q - a) * d) / (d * d)))) norm(q - (a + t * d));
|
||||
function _poly_dist(P, q) = let(n = len(P),
|
||||
inside = min([for (i = [0 : n - 1]) _cross(P[i], P[(i + 1) % n], q)]) >= 0)
|
||||
inside ? 0 : min([for (i = [0 : n - 1]) _seg_dist(P[i], P[(i + 1) % n], q)]);
|
||||
function _in_face(q) = _poly_dist(_ol_pts(), q) <= CORNER_R - EDGE_KEEP;
|
||||
function _right(q) = ((q - DIAG_POINT) * [cos(DIAG_ANGLE), -sin(DIAG_ANGLE)]) >= 0;
|
||||
function _rect_dist(c, size, q) = let(d = [max(abs(q[0] - c[0]) - size[0] / 2, 0), max(abs(q[1] - c[1]) - size[1] / 2, 0)]) norm(d);
|
||||
function _clear(q) = let(rr = HEX_OUT + HEX_GAP)
|
||||
_rect_dist(NX_WIN, NX_WINDOW + [2, 2] * NX_BEZEL, q) >= rr &&
|
||||
min([for (st = STICKS) norm(q - st)]) >= 53.5 / 2 + rr &&
|
||||
_rect_dist(HOOK_POS, [HOOK_BASE[0], HOOK_BASE[1] + 2 * HOOK_TRAVEL], q) >= rr &&
|
||||
_rect_dist(SWITCH_POS, [2 * 19 + 16, 7], q) >= rr;
|
||||
function _cell_ok(c) = _clear(c) && min([for (k = [0 : 5]) let(q = c + HEX_OUT * [cos(60 * k), sin(60 * k)])
|
||||
(_right(q) && _in_face(q)) ? 1 : 0]) == 1;
|
||||
HEX_CELLS = [for (i = [-30 : 30], j = [-20 : 20])
|
||||
let(c = [i * 1.5 * HEX_R, (j + (i % 2 == 0 ? 0 : 0.5)) * sqrt(3) * HEX_R]) if (_cell_ok(c)) c];
|
||||
// drop stray cells (fewer than 2 neighbours)
|
||||
HEX_KEEP = [for (c = HEX_CELLS) if (len([for (o = HEX_CELLS) if (o != c && norm(o - c) < 2 * HEX_R) 1]) >= 2) c];
|
||||
module hex_pattern_2d() for (c = HEX_KEEP) translate(c) difference() {
|
||||
circle(r = HEX_OUT, $fn = 6);
|
||||
circle(r = HEX_R - HEX_LINE / 2 / cos(30), $fn = 6);
|
||||
}
|
||||
module top_inlay_2d() { translate(SWITCH_POS) switch_text_2d(); hex_pattern_2d(); }
|
||||
// Two inlays, so each can have its own colour (owner: black case, red
|
||||
// honeycomb, white ON/OFF). Each extra colour adds a swap on the first 3 layers.
|
||||
module hex_inlay() translate([0, 0, -ENGRAVE_D]) lx(ENGRAVE_D) hex_pattern_2d();
|
||||
module text_inlay() translate([0, 0, -ENGRAVE_D]) lx(ENGRAVE_D) translate(SWITCH_POS) switch_text_2d();
|
||||
|
||||
// ---------- bottom shell ----------------------------------------------------
|
||||
|
||||
USBC_V = FRONT_V - 0.6; // receptacle front, 0.6 inside the front face
|
||||
|
||||
module bottom_shell() render(convexity = 10) difference() {
|
||||
union() {
|
||||
difference() {
|
||||
intersection() {
|
||||
outer_solid();
|
||||
translate([-BIG / 2, -BIG / 2, Z_BOT - 1]) cube([BIG, BIG, Z_SPLIT - Z_BOT + 1]);
|
||||
}
|
||||
inner_solid();
|
||||
}
|
||||
// locating lip
|
||||
translate([0, 0, Z_SPLIT - 3]) lx(3 + LIP_H) difference() {
|
||||
offset(delta = -(T + FIT)) outline_2d();
|
||||
offset(delta = -(T + FIT + LIP_W)) outline_2d();
|
||||
for (s = SCREWS) translate(s) circle(d = BOSS_D + 2 * FIT);
|
||||
}
|
||||
// ties the lip to the wall; its underside steps out at 45 deg (no flat overhang)
|
||||
for (i = [0 : 4]) translate([0, 0, Z_SPLIT - 3 - i * 0.35]) lx(3 + 0.01) difference() {
|
||||
offset(delta = -(T - JOIN)) outline_2d();
|
||||
offset(delta = -(T + FIT + LIP_W - i * 0.35)) outline_2d();
|
||||
for (s = SCREWS) translate(s) circle(d = BOSS_D + 2 * FIT);
|
||||
}
|
||||
intersection() {
|
||||
inner_solid();
|
||||
union() {
|
||||
// screw towers
|
||||
for (s = SCREWS) translate([s[0], s[1], Z_BOT]) cylinder(d = BOSS_D, h = Z_SPLIT - Z_BOT);
|
||||
// bail pivot bosses in the side walls, on pillars down to the floor (printable)
|
||||
for (m = [-1, 1]) hull() for (z = [BAIL_PIVOT[1], Z_BOT + 5])
|
||||
translate([m * SIDE_U, BAIL_PIVOT[0], z]) rotate([0, -m * 90, 0]) cylinder(d = 10, h = 14);
|
||||
// UPS standoffs
|
||||
for (m = [-1, 1]) translate([UPS_CENTRE[0] + m * UPS_HOLES, UPS_CENTRE[1], Z_BOT]) cylinder(d = 6, h = UPS_Z - Z_BOT);
|
||||
// kickstand recess and hinge trough walls
|
||||
kickstand_housing();
|
||||
// USB-C cradle
|
||||
usbc_cradle();
|
||||
}
|
||||
}
|
||||
// main board standoffs
|
||||
intersection() {
|
||||
inner_solid();
|
||||
perf_clamp_place() translate([0, 0, Z_BOT - PERF_Z]) cylinder(d = 8, h = PERF_Z - Z_BOT);
|
||||
}
|
||||
// front eyelet inserts: bosses behind the front wall
|
||||
intersection() {
|
||||
inner_solid();
|
||||
for (m = [-1, 1]) translate([m * HOOK_INSERTS, FRONT_V - T - 6 + JOIN, HOOK_FRONT_Z]) rotate([-90, 0, 0]) cylinder(d = 8, h = 6 + JOIN);
|
||||
}
|
||||
}
|
||||
// screw holes: counterbore from below, clearance up to the split
|
||||
for (s = SCREWS) translate([s[0], s[1], 0]) {
|
||||
translate([0, 0, Z_BOT - 1]) cylinder(d = HEAD_D, h = HEAD_Z - Z_BOT + 1);
|
||||
cylinder(d = SCREW_D, h = BIG, center = true);
|
||||
}
|
||||
for (m = [-1, 1]) translate([UPS_CENTRE[0] + m * UPS_HOLES, UPS_CENTRE[1], Z_BOT + 1.2]) cylinder(d = 2.5, h = 20);
|
||||
for (l = UPS_LEDS) translate(concat(UPS_CENTRE + l, Z_BOT - 1)) lx(T + 2) hull() for (d = [-2, 2]) translate([0, d]) circle(d = 2);
|
||||
perf_clamp_place() translate([0, 0, -6]) cylinder(d = INSERT_D, h = 7);
|
||||
for (m = [-1, 1]) translate([m * HOOK_INSERTS, FRONT_V - INSERT_L - 1, HOOK_FRONT_Z]) rotate([-90, 0, 0]) cylinder(d = INSERT_D, h = INSERT_L + 2);
|
||||
kickstand_cut();
|
||||
usbc_cut();
|
||||
for (m = [-1, 1]) translate([m * SIDE_U, BAIL_PIVOT[0], BAIL_PIVOT[1]]) rotate([0, -m * 90, 0]) translate([0, 0, -1]) cylinder(d = INSERT_D, h = 1 + 8);
|
||||
}
|
||||
|
||||
// Kickstand: a flap in a recess in the underside, hinged at the front on a
|
||||
// pin through a barrel. Friction on the pin holds the angle.
|
||||
KS_RECESS = KS_T + 0.5;
|
||||
KS_ROOF = Z_BOT + KS_RECESS + 2; // top of the recess roof
|
||||
KS_AXIS_Z = Z_BOT + KS_BARREL_D / 2; // barrel tangent to the underside (sits on the bed when printed)
|
||||
module kickstand_housing() {
|
||||
translate([-KS_W / 2 - FIT - KS_WALL, KS_V[0] - FIT - KS_WALL_V, Z_BOT])
|
||||
cube([KS_W + 2 * (FIT + KS_WALL), KS_V[1] - KS_V[0] + KS_BARREL_D / 2 + 2 * FIT + 2 * KS_WALL_V, KS_RECESS + 2]);
|
||||
translate([-KS_W / 2 - FIT - KS_WALL, KS_V[1] - KS_BARREL_D / 2 - FIT - KS_WALL_V, Z_BOT])
|
||||
cube([KS_W + 2 * (FIT + KS_WALL), KS_BARREL_D + 2 * (FIT + KS_WALL_V), KS_BARREL_D + 0.6 + FIT + 2]);
|
||||
}
|
||||
module kickstand_cut() {
|
||||
translate([-KS_W / 2 - FIT, KS_V[0] - FIT, Z_BOT - 1]) cube([KS_W + 2 * FIT, KS_V[1] - KS_V[0] + 2 * FIT, 1 + KS_RECESS]);
|
||||
translate([-KS_W / 2 - FIT, KS_V[1] - KS_BARREL_D / 2 - FIT, Z_BOT - 1]) cube([KS_W + 2 * FIT, KS_BARREL_D + 2 * FIT, 1 + KS_BARREL_D + 0.6 + FIT]);
|
||||
translate([0, KS_V[1], KS_AXIS_Z]) rotate([0, 90, 0]) cylinder(d = KS_PIN_D, h = KS_W + 2 * KS_WALL + 10, center = true);
|
||||
translate([KS_W / 2 + FIT + KS_WALL - 2.5, KS_V[1], KS_AXIS_Z]) rotate([0, 90, 0]) rotate([0, 0, 30]) cylinder(d = 5.5 / cos(30) + 0.3, h = 5, $fn = 6);
|
||||
}
|
||||
module kickstand() render(convexity = 10) difference() {
|
||||
union() {
|
||||
hull() {
|
||||
translate([-KS_W / 2, KS_V[0], Z_BOT]) cube([KS_W, 1, KS_T]);
|
||||
translate([-KS_W / 2, KS_V[1] - 1, Z_BOT]) cube([KS_W, 1, KS_T]);
|
||||
}
|
||||
translate([0, KS_V[1], KS_AXIS_Z]) rotate([0, 90, 0]) cylinder(d = KS_BARREL_D, h = KS_W, center = true);
|
||||
// finger notch lip at the free end is cut below
|
||||
}
|
||||
translate([0, KS_V[1], KS_AXIS_Z]) rotate([0, 90, 0]) cylinder(d = KS_PIN_D, h = KS_W + 2, center = true);
|
||||
translate([0, KS_V[0] - 2, Z_BOT - 1]) scale([1, 0.6, 1]) cylinder(r = 9, h = KS_T + 2); // finger notch
|
||||
}
|
||||
|
||||
module perf_place() translate(concat(PERF_POS, PERF_Z)) rotate([0, 0, -90]) children();
|
||||
// at each clamp post's axis, Z = 0 on the board's underside
|
||||
module perf_clamp_place() perf_place() for (c = PERF_CLAMPS)
|
||||
translate([c[0] + (c[0] > PERF[0] / 2 ? 1 : -1) * PERF_CLAMP_OUT, c[1], 0]) children();
|
||||
module perf_clamp_screws() perf_clamp_place() translate([0, 0, PERF[2]]) color("#b0b0b0") {
|
||||
cylinder(d = 7, h = 0.5); // washer
|
||||
translate([0, 0, 0.5]) cylinder(d = 5.7, h = 1.65); // M3 button head
|
||||
}
|
||||
|
||||
// USB-C breakout on a block, the receptacle in a slot through the front wall.
|
||||
module usbc_cradle() translate([USBC_POS[0] - USBC_BOARD[0] / 2 - 2, USBC_V - USBC_BOARD[1] - 2, Z_BOT])
|
||||
cube([USBC_BOARD[0] + 4, USBC_BOARD[1] + 2 + 3, USBC_Z - Z_BOT]);
|
||||
module usbc_cut() {
|
||||
inner_v = FRONT_V - T;
|
||||
// board pocket (glue it in), open towards the inside; through the wall only the board's own slot
|
||||
translate([USBC_POS[0] - USBC_BOARD[0] / 2 - FIT, USBC_V - USBC_BOARD[1] - FIT, USBC_Z]) cube([USBC_BOARD[0] + 2 * FIT, inner_v - (USBC_V - USBC_BOARD[1] - FIT), 20]);
|
||||
translate([USBC_POS[0] - USBC_BOARD[0] / 2 - FIT, USBC_V - 5, USBC_Z]) cube([USBC_BOARD[0] + 2 * FIT, 5 + FIT, USBC_BOARD[2] + 2 * FIT]);
|
||||
// receptacle opening, chamfered on the outside for the plug
|
||||
translate([USBC_POS[0], USBC_V - 3, USBC_Z + USBC_BOARD[2] + USBC_RCPT[2] / 2]) rotate([-90, 0, 0]) {
|
||||
linear_extrude(10) offset(r = 1.4) square([USBC_RCPT[0] + 0.6 - 2.8, USBC_RCPT[2] + 0.6 - 2.8], center = true);
|
||||
translate([0, 0, 3 + 0.6 - 0.01]) linear_extrude(1.01, scale = [1.3, 1.6]) offset(r = 1.4) square([USBC_RCPT[0] + 0.6 - 2.8, USBC_RCPT[2] + 0.6 - 2.8], center = true);
|
||||
}
|
||||
}
|
||||
|
||||
// Strap eyelet: printed base down (the d8 loop hole is a short bridge).
|
||||
module eyelet() render(convexity = 10) difference() {
|
||||
b = HOOK_BASE;
|
||||
union() {
|
||||
translate([-b[0] / 2, -b[1] / 2, 0]) cube(b);
|
||||
hull() {
|
||||
translate([-4, -b[1] / 2 + 2, 0]) cube([8, b[1] - 4, b[2]]);
|
||||
translate([-4, 0, HOOK_H - b[1] / 2 + 2]) rotate([0, 90, 0]) cylinder(r = b[1] / 2 - 2, h = 8);
|
||||
}
|
||||
}
|
||||
translate([-10, 0, HOOK_H - b[1] / 2 + 2]) rotate([0, 90, 0]) cylinder(d = HOOK_HOLE_D, h = 20);
|
||||
for (m = [-1, 1]) translate([m * HOOK_INSERTS, 0, -1]) hull() for (d = [-1, 1]) translate([0, d * HOOK_TRAVEL, 0]) cylinder(d = SCREW_D, h = 10);
|
||||
// screw heads (M3 button head, 5.7 mm) in slotted counterbores beside the loop
|
||||
for (m = [-1, 1]) translate([m * HOOK_INSERTS, 0, 2]) hull() for (d = [-1, 1]) translate([0, d * HOOK_TRAVEL, 0]) cylinder(d = 6.2, h = 10);
|
||||
}
|
||||
module eyelet_placed(shift = 0) translate([HOOK_POS[0], HOOK_POS[1] + shift, 0]) eyelet();
|
||||
module eyelet_front_placed() translate([HOOK_POS[0], FRONT_V, HOOK_FRONT_Z]) rotate([-90, 0, 0]) eyelet();
|
||||
|
||||
// Brace bail, in its pivot frame: arms along +y (folded forward), pivots on x.
|
||||
BAIL_X = SIDE_U + BAIL_GAP;
|
||||
module bail() render(convexity = 10) difference() {
|
||||
union() {
|
||||
for (m = [-1, 1]) hull() for (y = [0, BAIL_L]) translate([m > 0 ? BAIL_X : -BAIL_X - BAIL_T, y, 0]) rotate([0, 90, 0]) cylinder(d = BAIL_H, h = BAIL_T);
|
||||
// crossbar, bowed outwards
|
||||
n = 16;
|
||||
w = BAIL_X + BAIL_T / 2;
|
||||
translate([0, 0, -BAIL_H / 2]) lx(BAIL_H) for (i = [0 : n - 1]) hull() for (k = [i, i + 1]) {
|
||||
x = -w + k * 2 * w / n;
|
||||
translate([x, BAIL_L + BAIL_BOW * (1 - pow(x / w, 2))]) circle(d = 8);
|
||||
}
|
||||
}
|
||||
rotate([0, 90, 0]) cylinder(d = SCREW_D, h = 2 * BIG, center = true);
|
||||
}
|
||||
module bail_placed() translate([0, BAIL_PIVOT[0], BAIL_PIVOT[1]]) rotate([BAIL_ANGLE, 0, 0]) bail();
|
||||
|
||||
// ---------- reference parts (local frames, then placed) ---------------------
|
||||
|
||||
module nextion() {
|
||||
translate(concat(NX_CENTRE, NX_BOARD_TOP - 1.6)) color("#1e5a2a") difference() {
|
||||
translate(concat(-NX_BOARD / 2, 0)) cube(concat(NX_BOARD, 1.6));
|
||||
for (du = [-1, 1], dv = [-1, 1]) translate([du * NX_HOLES[0], dv * NX_HOLES[1], 0] / 2 + [0, 0, -1]) cylinder(d = 3.2, h = 4);
|
||||
}
|
||||
translate(concat(NX_CENTRE - NX_LCD / 2, NX_BOARD_TOP)) color("#202020") cube(concat(NX_LCD, NX_LCD_T));
|
||||
translate(concat(NX_WIN - [73.44, 48.96] / 2, -NX_BEZEL - 0.2)) color("#3050a0") cube([73.44, 48.96, 0.2]);
|
||||
translate(concat(NX_CENTRE + [(NX_FLIP ? 1 : -1) * (NX_BOARD[0] / 2 - 7), -5], NX_BOARD_TOP - 1.6 - NX_BACK)) color("#eee") cube([6, 10, NX_BACK]); // J1 4-pin connector (approx.)
|
||||
}
|
||||
|
||||
module ups_18650() translate(concat(UPS_CENTRE, UPS_Z)) {
|
||||
color("#202020") difference() {
|
||||
translate([-89 / 2, -22 / 2, 0]) cube([89, 22, 1.6]);
|
||||
for (m = [-1, 1]) translate([m * UPS_HOLES, 0, -1]) cylinder(d = 3, h = 4);
|
||||
}
|
||||
color("#151515") translate([-77 / 2, -21 / 2, 1.6]) difference() {
|
||||
cube([77, 21, 22.4 - 1.6]);
|
||||
translate([3, 1.5, 3]) cube([71, 18, 30]);
|
||||
}
|
||||
color("#4060c0") translate([-65 / 2, 0, 1.6 + 3 + 18.5 / 2]) rotate([0, 90, 0]) cylinder(d = 18.5, h = 65);
|
||||
color("#b0b0b0") translate([89 / 2 - 7, -4.5, 1.6]) cube([7.5, 9, 3.2]); // Type-C (charge input)
|
||||
color("#808080") translate([89 / 2 + 0.5, -6, 0]) cube([18, 12, 7]); // cut-cable plug (approx.)
|
||||
}
|
||||
|
||||
// Main board as in the owner's photos (layout approximate).
|
||||
module main_board() { perf_clamp_screws(); main_board_only(); }
|
||||
module main_board_only() perf_place() {
|
||||
color("#2a8a4a") difference() {
|
||||
cube(PERF);
|
||||
}
|
||||
// Teensy 4.0 in female headers (8.5 mm), micro-USB at the board edge
|
||||
translate([12.1, 1, PERF[2]]) {
|
||||
color("#111") for (x = [0, 17.8 - 2.5]) translate([x, 0, 0]) cube([2.5, 35.6, 8.5]);
|
||||
translate([0, 0, 8.5]) {
|
||||
color("#1e5a2a") cube([17.8, 35.6, 1.6]);
|
||||
color("#222") translate([4, 12, 1.6]) cube([10, 10, 1.2]);
|
||||
color("#c0c0c0") translate([(17.8 - 7.5) / 2, -1, 1.6]) cube([7.5, 5.7, 2.9]);
|
||||
color("#808080") translate([(17.8 - 10) / 2, -1 - 18, 1.6 - 2]) cube([10, 18, 7]); // cable plug (approx.)
|
||||
}
|
||||
}
|
||||
// nRF24L01 lying over the far edge, on its 2 x 4 header
|
||||
translate([15, 63, PERF[2]]) {
|
||||
color("#111") translate([2, 0, 0]) cube([10, 5, 2.5]);
|
||||
color("#151515") translate([0, 0, 2.5]) cube([15.3, 29, 1.2]);
|
||||
}
|
||||
color("#222") translate([2, 74, PERF[2]]) cube([11, 6.3, 6.3]); // 100 uF cap, lying
|
||||
color("#222") translate([5, 58, PERF[2]]) cube([7, 6.5, 1.8]); // AMS1117
|
||||
// ribbon and display/IMU connectors at the edge, plugs on top (approx.)
|
||||
color("#333") translate([PERF[0] - 8, 8, PERF[2]]) cube([5, 21, 12]);
|
||||
color("#333") translate([PERF[0] - 8, 44, PERF[2]]) cube([5, 11, 12]);
|
||||
}
|
||||
|
||||
// GY-521 on the main board, on headers (board frame: 15.6 across, 21.2 along)
|
||||
module imu_gy521() perf_place() translate(concat(PERF_IMU, PERF[2])) {
|
||||
color("#111") translate([0, 1, 0]) cube([2.5, 19, 8.5]);
|
||||
translate([0, 0, 8.5]) {
|
||||
color("#1f4fa0") cube([15.6, 21.2, 1.6]);
|
||||
color("#222") translate([6, 8.6, 1.6]) cube([4, 4, 1]);
|
||||
}
|
||||
}
|
||||
|
||||
module usbc_breakout() translate([USBC_POS[0], USBC_V, USBC_Z]) {
|
||||
color("#1a7a3a") translate([-USBC_BOARD[0] / 2, -USBC_BOARD[1], 0]) cube(USBC_BOARD);
|
||||
color("#c0c0c0") translate([-USBC_RCPT[0] / 2, -USBC_RCPT[1], USBC_BOARD[2]]) cube(USBC_RCPT);
|
||||
}
|
||||
|
||||
// Slide switch as on the old remote: mounting plate on the top face, screwed
|
||||
// down with 2 screws, body hanging through the slot. Sizes approximate.
|
||||
module slide_switch() translate(SWITCH_POS) color("#303030") {
|
||||
translate([-12.5, -3.5, 0]) cube([25, 7, 0.8]); // mounting plate
|
||||
translate([-7.5, -3.5, -10]) cube([15, 7, 10]); // body, through the slot
|
||||
translate([-1.5, -1.5, 0.8]) cube([3, 3, 4]); // knob
|
||||
}
|
||||
|
||||
// JH-D300X-R1 3-axis joystick (owner's drawing): handle d35 with the twist
|
||||
// pot inside, dust cover d53.5 on top of the plate, 40.4 mm control box
|
||||
// (25.5 tall) under it, clamping the plate with 4 x M2.5 x 14. X/Y pots
|
||||
// d16.5 sticking 9.2 out of the box on -x and -y. Local Z = 0 on the cover's
|
||||
// underside (the top face); the box hangs from the plate's underside.
|
||||
JOY_BOX = 40.4; JOY_BOX_H = 25.5; JOY_POT_D = 16.5; JOY_POT_L = 49.6 - 40.4;
|
||||
JOY_COVER_T = 3.8; JOY_POT_Z = 15.3; // pot axis above the box bottom (from the drawing)
|
||||
module jh_d300x() {
|
||||
zb = -PLATE - JOY_BOX_H; // box bottom
|
||||
color("#202020") {
|
||||
difference() {
|
||||
cylinder(d1 = 53.5, d2 = 50, h = JOY_COVER_T);
|
||||
for (du = [-1, 1], dv = [-1, 1]) translate([du * STICK_SCREW, dv * STICK_SCREW, -1]) cylinder(d = 2.7, h = 6);
|
||||
}
|
||||
translate([0, 0, JOY_COVER_T]) cylinder(d = 39.5, h = 1); // dust ring
|
||||
translate([-JOY_BOX / 2, -JOY_BOX / 2, zb]) cube([JOY_BOX, JOY_BOX, JOY_BOX_H]);
|
||||
translate([0, 0, zb + JOY_POT_Z]) {
|
||||
rotate([0, -90, 0]) translate([0, 0, JOY_BOX / 2]) cylinder(d = JOY_POT_D, h = JOY_POT_L); // Y pot, -x
|
||||
rotate([90, 0, 0]) translate([0, 0, JOY_BOX / 2]) cylinder(d = JOY_POT_D, h = JOY_POT_L); // X pot, -y
|
||||
}
|
||||
t = JOY_COVER_T;
|
||||
translate([0, 0, t]) cylinder(d = 6, h = 11.5); // shaft
|
||||
translate([0, 0, t + 11]) cylinder(d1 = 12, d2 = 18, h = 8); // neck
|
||||
translate([0, 0, t + 19]) cylinder(d = 32, h = 7);
|
||||
translate([0, 0, t + 26]) cylinder(d = 35, h = 19); // knurled grip
|
||||
translate([0, 0, t + 45]) cylinder(d1 = 35, d2 = 30, h = 8); // top: 53 above the cover
|
||||
}
|
||||
}
|
||||
// pots towards the middle and towards the user
|
||||
module sticks() for (s = STICKS) translate(concat(s, 0)) if (s[0] > 0) jh_d300x(); else mirror([1, 0, 0]) jh_d300x();
|
||||
|
||||
module electronics() {
|
||||
nextion();
|
||||
ups_18650();
|
||||
main_board();
|
||||
imu_gy521();
|
||||
usbc_breakout();
|
||||
slide_switch();
|
||||
}
|
||||
|
||||
// ---------- assembly and print layout --------------------------------------
|
||||
|
||||
module kickstand_placed() translate([0, KS_V[1], KS_AXIS_Z]) rotate([KS_OPEN, 0, 0]) translate([0, -KS_V[1], -KS_AXIS_Z]) kickstand();
|
||||
|
||||
module remote_v2() {
|
||||
if (SHOW_BOTTOM) { color("#1c1c1c") bottom_shell(); color("#1c1c1c") kickstand_placed(); color("#1c1c1c") bail_placed(); }
|
||||
if (HOOK_MOUNT == "front") color("#1c1c1c") eyelet_front_placed();
|
||||
else if (SHOW_TOP) translate([0, 0, EXPLODE]) color("#1c1c1c") eyelet_placed();
|
||||
if (SHOW_TOP) translate([0, 0, EXPLODE]) { color("#262626") top_shell(); color("#c8102e") hex_inlay(); color("#f2f2f2") text_inlay(); }
|
||||
if (SHOW_PARTS) { translate([0, 0, EXPLODE]) { nextion(); slide_switch(); }
|
||||
ups_18650(); main_board(); imu_gy521(); usbc_breakout(); }
|
||||
if (SHOW_STICKS) translate([0, 0, EXPLODE]) sticks();
|
||||
}
|
||||
|
||||
if (PART == "assembly") remote_v2();
|
||||
else if (PART == "top") rotate([180, 0, 0]) top_shell(); // face down on the bed
|
||||
else if (PART == "inlay") rotate([180, 0, 0]) hex_inlay(); // load with "top" (and "text") as one multi-part object
|
||||
else if (PART == "text") rotate([180, 0, 0]) text_inlay();
|
||||
else if (PART == "bottom") translate([0, 0, -Z_BOT]) bottom_shell();
|
||||
else if (PART == "kickstand") translate([0, 0, -Z_BOT]) kickstand();
|
||||
else if (PART == "eyelet") eyelet(); // base on the bed, the loop hole bridges
|
||||
else if (PART == "bail") translate([0, 0, BAIL_H / 2]) bail();
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,6 +1,6 @@
|
||||
; Hexapod: two firmware images in one project.
|
||||
; pio run -e robot build robot/ (Teensy 3.6)
|
||||
; pio run -e remote build remote/ (Teensy 4.1)
|
||||
; pio run -e remote build remote/ (Teensy 4.0)
|
||||
; pio run -e robot -t upload flash (Teensy Loader / teensy_cli)
|
||||
; pio device monitor -e robot serial monitor at 115200
|
||||
;
|
||||
@@ -25,5 +25,5 @@ board = teensy36
|
||||
custom_src_dir = robot
|
||||
|
||||
[env:remote]
|
||||
board = teensy41
|
||||
board = teensy40
|
||||
custom_src_dir = remote
|
||||
@@ -0,0 +1,11 @@
|
||||
#include "Battery.h"
|
||||
#include "Config.h"
|
||||
|
||||
void Battery::update(float dt) {
|
||||
if (BATTERY_SENSE_PIN == 255) return; // not fitted: stays 0
|
||||
// Sticks set the ADC to ADC_BITS for every analogRead().
|
||||
float v = analogRead(BATTERY_SENSE_PIN) * (BATTERY_ADC_REF_V / ((1 << ADC_BITS) - 1)) * BATTERY_DIVIDER;
|
||||
if (!primed_) { volts_ = v; primed_ = true; return; }
|
||||
float a = constrain(dt / 2.0f, 0.0f, 1.0f); // ~2 s time constant
|
||||
volts_ += (v - volts_) * a;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Handset battery voltage through a resistor divider (Config.h BATTERY_*), smoothed.
|
||||
class Battery {
|
||||
public:
|
||||
void update(float dt);
|
||||
uint16_t millivolts() const { return (uint16_t)(volts_ * 1000.0f + 0.5f); } // 0 = not fitted
|
||||
|
||||
private:
|
||||
float volts_ = 0;
|
||||
bool primed_ = false;
|
||||
};
|
||||
@@ -2,7 +2,7 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
// =====================================================================
|
||||
// Remote handset config (Teensy 4.1). Everything here is a placeholder
|
||||
// Remote handset config (Teensy 4.0). 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.
|
||||
@@ -31,16 +31,18 @@ constexpr uint32_t LINK_LOST_MS = 300;
|
||||
// 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 };
|
||||
|
||||
// A0-A5 in a row, as wired for the old remote (2019-teensy-nrf24/remote/Joysticks.h).
|
||||
// A4/A5 are also Wire's SDA/SCL - that's why the MPU6050 is on Wire2 below.
|
||||
// BENCH: which physical axis is on which pin wasn't recoverable from the old
|
||||
// code; reorder these so each lands on the right logical axis ('p' dump).
|
||||
constexpr uint8_t STICK_PINS[NUM_AXES] = { A0, A1, A2, A3, A4, A5 };
|
||||
// The old remote had A0-A5 in a row (2019-teensy-nrf24/remote/Joysticks.h).
|
||||
// The two wires on A4/A5 (pins 18/19) move to A7/A8 (pins 21/22), because
|
||||
// 18/19 are Wire's SDA/SCL for the MPU6050 (owner's choice 2026-11-17: edge
|
||||
// headers only, no pads underneath). A6 (20) stays free for the battery.
|
||||
// Checked on the real remote (2026-11-17): the left stick is on A3/A7/A8, the
|
||||
// right stick on A0/A1/A2.
|
||||
constexpr uint8_t STICK_PINS[NUM_AXES] = { A3, A7, A8, A0, A1, A2 };
|
||||
|
||||
// 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 };
|
||||
// Checked on the real remote (2026-11-17): every axis but the right twist reads inverted.
|
||||
constexpr bool STICK_INVERT[NUM_AXES] = { true, true, true, true, true, false };
|
||||
|
||||
constexpr int ADC_BITS = 12; // 0..4095
|
||||
constexpr int ADC_AVERAGING = 8; // hardware averaging per analogRead()
|
||||
@@ -69,15 +71,14 @@ constexpr float STICK_EXPO[NUM_AXES] = { 0.25f, 0.25f, 0.30f, 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 the second I2C bus (Wire2: SDA2 = 25, SCL2 = 24) ----
|
||||
// Not Wire (18/19): those pins are stick axes A4/A5.
|
||||
#define IMU_WIRE Wire2
|
||||
// ---- Remote IMU: MPU6050 on Wire (SDA = 18, SCL = 19) ----
|
||||
// Edge-header pins; the stick axes that used to be there moved to A7/A8.
|
||||
#define IMU_WIRE Wire
|
||||
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.
|
||||
// Checked on the real remote (2026-11-17): the defaults are right - tipping
|
||||
// the far edge down reads +pitch, dipping the right side reads +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
|
||||
@@ -107,24 +108,37 @@ constexpr uint32_t WALK_LINGER_MS = 250;
|
||||
// (post-deadzone, -1000..1000 scale), so the robot never arms mid-command.
|
||||
constexpr int16_t ARM_CENTER_TOLERANCE = 50;
|
||||
|
||||
// ---- Nextion touchscreen on Serial1 (RX1 = pin 0, TX1 = pin 1), as in the old remote ----
|
||||
#define NEXTION_SERIAL Serial1
|
||||
// BENCH: the old sketch used the ITEAD library, whose nexInit() talks at 9600
|
||||
// unless it was modified. This firmware expects 115200: add `bauds=115200`
|
||||
// to the HMI's Program.s (see remote/NEXTION.md) or set 9600 here.
|
||||
// ---- Nextion touchscreen on Serial2 (RX2 = pin 7, TX2 = pin 8) ----
|
||||
// Found on the real remote (2026-11-17): the display answers on Serial2 at 115200. The old
|
||||
// sketch's Serial1.begin() was a red herring; its ITEAD Nextion library used Serial2.
|
||||
#define NEXTION_SERIAL Serial2
|
||||
// The HMI (remote/remote.HMI) sets this with `bauds=115200` in Program.s.
|
||||
constexpr uint32_t NEXTION_BAUD = 115200;
|
||||
// Controls live on this page (remote/NEXTION.md). Its components must have
|
||||
// vscope = global, so the firmware can update them whichever page is showing.
|
||||
#define NEXTION_CONTROL_PAGE "control"
|
||||
// The old HMI's link-status texts (pages 1 and 3, both named t1), also
|
||||
// vscope = global. BENCH: use your real page names (Nextion's default
|
||||
// names are page0..page5; only page 4 "uploadscreen" is known).
|
||||
constexpr const char* NEXTION_STATUS_TEXTS[] = { "page1.t1", "page3.t1" };
|
||||
// The old HMI's "upload" button: page 1, component id 5 (b2), sends the
|
||||
// standard touch event. It puts the remote into Nextion upload passthrough.
|
||||
constexpr uint8_t NEXTION_UPLOAD_BUTTON_PAGE = 1;
|
||||
constexpr uint8_t NEXTION_UPLOAD_BUTTON_ID = 5;
|
||||
#define NEXTION_UPLOAD_PAGE "uploadscreen"
|
||||
#define NEXTION_UPLOAD_PAGE "upload"
|
||||
// The firmware only writes the page that is showing (each page reports itself
|
||||
// with a 'P' frame), and only what changed since the last write.
|
||||
constexpr uint32_t NEXTION_RENDER_MS = 50; // repaint rate of the showing page
|
||||
constexpr uint32_t NEXTION_SERVOS_RENDER_MS = 100; // SERVOS page: 36 values, so a bit slower
|
||||
constexpr uint32_t NEXTION_FULL_REFRESH_MS = 2000; // re-push everything periodically (self-heals after a display reset)
|
||||
constexpr uint32_t NEXTION_LINK_REFRESH_MS = 250; // link-quality text update rate
|
||||
// A render is skipped while less than this is free in the TX buffer, so the
|
||||
// TX loop never blocks on the display.
|
||||
constexpr int NEXTION_TX_MIN_FREE = 400;
|
||||
constexpr uint32_t STATUS_MESSAGE_MS = 2500; // how long transient status messages stay up
|
||||
constexpr uint32_t UPLOAD_CONFIRM_MS = 3000; // UPDATE DISPLAY: second tap within this
|
||||
|
||||
// ---- Robot telemetry (ack payloads) ----
|
||||
constexpr uint32_t TELEMETRY_STALE_MS = 500; // older than this -> the display shows "no data"
|
||||
|
||||
// ---- Servo centre calibration (CAL page) ----
|
||||
constexpr int16_t SERVO_TRIM_STEP_SMALL_US = 1;
|
||||
constexpr int16_t SERVO_TRIM_STEP_BIG_US = 10;
|
||||
// SAVE / REVERT go out for this long (the robot acts once; repeats are harmless).
|
||||
constexpr uint32_t SERVO_CAL_OP_MS = 300;
|
||||
|
||||
// ---- Battery voltage (STATUS page) ----
|
||||
// BENCH: not built yet. Planned: the 18650's + through a 100k/100k divider
|
||||
// into A6 (pin 20) with 100 nF to GND (see README.md "Planned"). Then set
|
||||
// the pin to A6. 255 = not fitted: the display shows "--".
|
||||
constexpr uint8_t BATTERY_SENSE_PIN = 255;
|
||||
constexpr float BATTERY_DIVIDER = 2.0f; // battery V / pin V
|
||||
constexpr float BATTERY_ADC_REF_V = 3.3f;
|
||||
@@ -1,99 +1,107 @@
|
||||
# Nextion HMI build guide
|
||||
# Nextion HMI
|
||||
|
||||
Start from your existing HMI (`../../2019-teensy-nrf24/remote-nextion/remote.HMI`, the servo
|
||||
monitor) and **add one page named `control`** with the components below.
|
||||
Your existing pages keep working:
|
||||
`remote.HMI` in this folder is the touchscreen project (NX4832T035, 480×320),
|
||||
turned 180° (Editor display direction 270) because the display sits upside
|
||||
down in the remote.
|
||||
It's **generated** by `../scripts/make_remote_hmi.py`: change the layout
|
||||
there and re-run it, rather than editing the `.HMI` in Nextion Editor (an
|
||||
Editor edit is lost on the next run). Then open `remote.HMI` in Nextion
|
||||
Editor, compile, and upload the `.tft`: SETUP › UPDATE DISPLAY (tap twice)
|
||||
or serial key `U` puts the remote in USB passthrough for the upload. In the
|
||||
Editor, upload to the Teensy's COM port with the baud rate on auto. When the
|
||||
Editor is newer than the display's firmware, the upload first updates that
|
||||
firmware: don't interrupt it. Power-cycle the remote afterwards to leave
|
||||
upload mode.
|
||||
|
||||
| Existing | What the firmware does with it |
|
||||
|----------|--------------------------------|
|
||||
| page 1 `t1`, page 3 `t1` | writes `connected` / `not connected`, as the old firmware did. Set both to **vscope = global**, and put their real names (`page1.t1`, …) in `NEXTION_STATUS_TEXTS` in `Config.h` |
|
||||
| page 1 `b2` (id 5) | **upload mode**: keep "Send Component ID" ticked on it. The remote shows `uploadscreen` and passes USB straight through to the display, so Nextion Editor can upload over the remote's USB port. Power-cycle to leave. Serial key `U` does the same |
|
||||
| page 4 `uploadscreen` | shown on entering upload mode |
|
||||
| page 3 sliders `h0`–`h17` | not driven yet: the robot doesn't send servo positions back (that needs the planned telemetry channel) |
|
||||
|
||||
The firmware drives the display by **component name**, and the display
|
||||
talks back with small custom frames sent from each button's touch event.
|
||||
Layout, fonts and colors are up to you. The names and event code must
|
||||
match exactly.
|
||||
|
||||
**Set every component on `control` to vscope = global.** The firmware
|
||||
writes them as `control.<name>` and keeps them current whichever page is
|
||||
showing. A local component on another page can't be written from outside,
|
||||
and with `bkcmd=0` the display ignores those writes silently.
|
||||
|
||||
The remote is authoritative. Use **plain Button** components (not
|
||||
Dual-state buttons) and don't toggle anything locally in the HMI. The
|
||||
remote repaints every label and color from its own state, including a
|
||||
full redraw every 2 s and whenever the display reboots.
|
||||
|
||||
## Program.s / page setup
|
||||
|
||||
In **Program.s**, before `page 0`, add:
|
||||
The display is on the remote's **Serial2** (Teensy RX2 = pin 7, TX2 = pin 8),
|
||||
at 115200.
|
||||
|
||||
```
|
||||
bauds=115200
|
||||
python scripts\make_remote_hmi.py (from 2026-current)
|
||||
```
|
||||
|
||||
This sets the display to the `NEXTION_BAUD` in `remote/Config.h`. The
|
||||
display ships at 9600, and `bauds=` persists across power cycles. The old
|
||||
firmware used the ITEAD library at 9600, so your HMI probably runs at 9600
|
||||
today.
|
||||
It needs the `tools/nextion-hmi-tools` submodule, Pillow, DejaVu Sans Mono
|
||||
(found in matplotlib, or `--font-dir`), and the old remote HMI
|
||||
(`../../2019-teensy-nrf24/remote-nextion/remote.HMI`, read only) for the
|
||||
splash picture. To look at a page without the Editor, use the `nxhmi` MCP
|
||||
server's `render_page`.
|
||||
|
||||
Name the new page `control`. The firmware never changes pages, except
|
||||
into `uploadscreen` for upload mode. Add buttons on your other pages to
|
||||
switch to it (`page control`).
|
||||
## Pages
|
||||
|
||||
## Components
|
||||
| id | Page | Contents |
|
||||
|----|------|----------|
|
||||
| 0 | `splash` | spider picture, 1.5 s, then `drive` |
|
||||
| 1 | `drive` | ARM, SIT/STAND, gait, mode, right-stick mode, both gyros, speed + height sliders, messages |
|
||||
| 2 | `status` | remote and robot IMU roll/pitch, the 6 stick axes, both batteries, radio |
|
||||
| 3 | `servos` | all 18 joint angles (robot telemetry), laid out as a top view of the robot |
|
||||
| 4 | `cal` | servo centre calibration: START/STOP, leg and joint, trim ±1/±10 µs, ZERO, REVERT, SAVE |
|
||||
| 5 | `setup` | stick calibration, LEVEL REMOTE, UPDATE DISPLAY, backlight, versions |
|
||||
| 6 | `upload` | shown during display update mode |
|
||||
|
||||
Text components need **txt_maxl** at least as large as the longest string
|
||||
the firmware writes. Button bounds here allow some slack.
|
||||
Pages 1–5 share a frame: a top bar (`tLink` link quality, `tState` robot
|
||||
state + battery, red `bKill` DISARM) and tab buttons `bTab0`–`bTab4` that
|
||||
switch pages locally (`page drive`, ...). The page order is the page id, and
|
||||
must match `UiPage` in `UiTypes.h`.
|
||||
|
||||
| Name | Type | txt_maxl | Firmware writes |
|
||||
|----------|---------|----------|----------------------------------------------|
|
||||
| `bArm` | Button | 10 | `ARMED` / `DISARMED`, bco red / gray |
|
||||
| `bKill` | Button | – | nothing (static "DISARM" label, make it red) |
|
||||
| `bStand` | Button | 6 | `STAND` / `SIT`, bco green / gray |
|
||||
| `bGait` | Button | 8 | `TRIPOD` / `WAVE` |
|
||||
| `bMode` | Button | 8 | `DIRECT` / `HEADING` |
|
||||
| `bRs` | Button | 10 | `R: ROTATE` / `R: MOVE` |
|
||||
| `bRGyro` | Button | – | bco green / gray (label it "REMOTE GYRO") |
|
||||
| `bBGyro` | Button | – | bco green / gray (label it "ROBOT GYRO") |
|
||||
| `hSpeed` | Slider | – | `val` 0..255 (set **maxval = 255**) |
|
||||
| `nSpeed` | Number | – | `val` 0..100 (speed %) |
|
||||
| `hHeight`| Slider | – | `val` 0..200, 100 = default (set **maxval = 200**) |
|
||||
| `nHeight`| Number | – | `val` -100..100 (height offset %) |
|
||||
| `bCal` | Button | 9 | `CAL` / `SAVE CAL`, bco orange while active |
|
||||
| `bCalX` | Button | – | nothing (static "CANCEL CAL") |
|
||||
| `bZero` | Button | – | nothing (static "LEVEL") |
|
||||
| `tLink` | Text | 10 | `LINK 97%` / `NO LINK`, pco green / red |
|
||||
| `tMsg` | Text | 40 | status / rejection messages |
|
||||
## How the firmware talks to it
|
||||
|
||||
## Touch events
|
||||
- **Display → remote:** every touch sends a custom frame with `printh`:
|
||||
`'#'` (0x23), length, command byte, optional value byte. "Send Component
|
||||
ID" is off everywhere.
|
||||
- **Page tracking:** each page's Preinitialize event sends `'P'` + its page
|
||||
id, so the remote always knows which page is showing. After a remote
|
||||
restart it asks with `sendme`.
|
||||
- **Remote → display:** the remote writes only the showing page, by local
|
||||
component name (no `vscope = global`, no display RAM spent), and only
|
||||
what changed (`NextionUI` hashes every instruction). Everything is
|
||||
re-sent every 2 s and after the display restarts (0x88), so a dropped
|
||||
byte heals itself. Render rate is 20 Hz (SERVOS 10 Hz), and a render is
|
||||
skipped rather than blocking when the TX buffer is full.
|
||||
- **The remote is authoritative:** plain buttons only, nothing toggles
|
||||
locally. Exceptions that are purely local: the tabs, and the backlight
|
||||
slider (`dim` / `dims`).
|
||||
- `Program.s` sets `bauds=115200` (= `NEXTION_BAUD` in `Config.h`), stored in the display.
|
||||
|
||||
Put this in each component's **Touch Release Event** (the Speed slider
|
||||
uses **Touch Move** as well so the value tracks your finger). Frame format:
|
||||
`'#'` (0x23), length, command byte, optional value byte.
|
||||
## Commands (display → remote)
|
||||
|
||||
| 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 |
|
||||
| `hHeight` | `printh 23 02 48`<br>`prints hHeight.val,1` | `H` + height byte (Touch Move + Release) |
|
||||
| `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 |
|
||||
| Component | Event code | Command |
|
||||
|-----------|-----------|---------|
|
||||
| every page | `printh 23 02 50 <id>` (Preinitialize) | `P` page shown |
|
||||
| `bKill` | `printh 23 01 4B` | `K` disarm, always accepted |
|
||||
| `bArm` | `printh 23 01 41` | `A` toggle arm (needs sticks centered) |
|
||||
| `bStand` | `printh 23 01 53` | `S` toggle sit/stand |
|
||||
| `bGait` | `printh 23 01 47` | `G` tripod ↔ wave |
|
||||
| `bMode` | `printh 23 01 4D` | `M` direct ↔ heading-hold |
|
||||
| `bRs` | `printh 23 01 58` | `X` right stick rotate ↔ translate |
|
||||
| `bRGyro` | `printh 23 01 52` | `R` remote-gyro tilt on/off |
|
||||
| `bBGyro` | `printh 23 01 42` | `B` robot self-stabilize on/off |
|
||||
| `hSpeed` | `printh 23 02 56` + `prints hSpeed.val,1` (move + release) | `V` + speed 0..255 |
|
||||
| `hHeight` | `printh 23 02 48` + `prints hHeight.val,1` (move + release) | `H` + height 0..200, 100 = default |
|
||||
| `bCal` | `printh 23 01 43` | `C` start / save stick calibration |
|
||||
| `bCalX` | `printh 23 01 51` | `Q` cancel stick calibration |
|
||||
| `bZero` | `printh 23 01 5A` | `Z` zero handset level |
|
||||
| `bUpd` | `printh 23 01 55` | `U` display update mode (second tap within 3 s) |
|
||||
| `bCalOn` | `printh 23 01 4F` | `O` servo calibration start/stop |
|
||||
| `bL0`–`bL5` | `printh 23 02 4C 0n` | `L` + leg (0 FR, 1 MR, 2 RR, 3 RL, 4 ML, 5 FL) |
|
||||
| `bJ0`–`bJ2` | `printh 23 02 4A 0n` | `J` + joint (0 coxa, 1 femur, 2 tibia) |
|
||||
| `bM10` `bM1` `bP1` `bP10` `bZeroT` | `printh 23 02 54 0n` | `T` + trim step: 0 −10, 1 −1, 2 +1, 3 +10, 4 zero (µs) |
|
||||
| `bSave` | `printh 23 01 57` | `W` save trims on the robot |
|
||||
| `bRevert` | `printh 23 01 45` | `E` reload trims from the robot's EEPROM |
|
||||
|
||||
**Uncheck "Send Component ID"** on every component on `control`: those
|
||||
standard touch frames only add serial traffic. The one exception is the old
|
||||
page 1 `b2` upload button (see the top).
|
||||
To add a control: pick an unused command byte, add it to
|
||||
`NextionUI::decodeCustom()` and `UiCmd`, handle it in `RemoteState::handle()`,
|
||||
add the component in `make_remote_hmi.py`, and a row here.
|
||||
|
||||
Protocol details and the parser live in `NextionUI.h/.cpp`. To add a
|
||||
control, pick an unused command byte, add it to `NextionUI::decodeCustom()`
|
||||
and `UiCmd`, handle it in `RemoteState::handle()`, and add a row here.
|
||||
## What the remote writes (remote → display)
|
||||
|
||||
| Page | Components |
|
||||
|------|-----------|
|
||||
| all but splash/upload | `tLink` txt/pco, `tState` txt/pco |
|
||||
| drive | `bArm` `bStand` txt/bco, `bGait` `bMode` `bRs` txt, `bRGyro` `bBGyro` bco, `hSpeed` `hHeight` val, `tSpeed` `tHeight` `tMsg` txt |
|
||||
| status | `tRRoll` `tRPitch` `tBRoll` `tBPitch` txt, `hRRoll` `hRPitch` `hBRoll` `hBPitch` val (0..90 = ±45°), `hLX` `hLY` `hLT` `hRX` `hRY` `hRT` val (0..200 = ±1000), `tBatt` `tRadio` txt. The indicator sliders have touch switched off (`tsw` in Preinitialize) |
|
||||
| servos | `tJ0`–`tJ17` txt (index = leg × 3 + joint), `jJ0`–`jJ17` val (position in the servo's travel), `tSrv` txt/pco |
|
||||
| cal | `bCalOn` txt/bco, `bL0`–`bL5` `bJ0`–`bJ2` bco (selected = green), `tTrim` txt, `bSave` bco (orange = unsaved), `tMsg` txt |
|
||||
| setup | `bCal` txt/bco, `bUpd` txt/bco, `tVer` txt, `tMsg` txt |
|
||||
|
||||
`txt_maxl` of each text is set in the generator to fit the longest string
|
||||
the firmware writes. Keep them in step when you change a format string.
|
||||
@@ -6,27 +6,29 @@ static constexpr uint16_t COL_ON = 2016; // green
|
||||
static constexpr uint16_t COL_OFF = 50712; // Nextion default light gray
|
||||
static constexpr uint16_t COL_DANGER = 63488; // red
|
||||
static constexpr uint16_t COL_WARN = 64512; // orange
|
||||
static constexpr uint16_t COL_TEXT = 65535; // white (top bar text)
|
||||
static constexpr uint16_t COL_DIM = 33808; // gray (no data)
|
||||
|
||||
// The default 64-byte TX buffer would make a full redraw (~400 bytes at
|
||||
// 115200 baud) block the TX loop for ~35 ms. Give it room to queue.
|
||||
static uint8_t tx_extra[1024];
|
||||
// The default 64-byte TX buffer would make a full redraw (~1 kB at 115200
|
||||
// baud) block the TX loop for ~90 ms. Give it room to queue.
|
||||
static uint8_t tx_extra[2048];
|
||||
static uint8_t rx_extra[128];
|
||||
|
||||
// Every control lives on NEXTION_CONTROL_PAGE with vscope = global, so it's
|
||||
// addressed as page.component and can be updated whichever page is showing.
|
||||
#define P NEXTION_CONTROL_PAGE "."
|
||||
static const char* const LEG_NAMES[6] = { "FR", "MR", "RR", "RL", "ML", "FL" }; // robot leg order
|
||||
static const char* const JOINT_NAMES[3] = { "coxa", "femur", "tibia" };
|
||||
|
||||
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 (and a
|
||||
// write to a page the HMI doesn't have is then just ignored)
|
||||
cmd("bkcmd=0"); // no success/fail replies - we never need them (and a write
|
||||
// to a component that isn't on the showing page is ignored)
|
||||
cmd("sendme"); // which page is showing? (the display may have kept running)
|
||||
force_full_ = true;
|
||||
}
|
||||
|
||||
void NextionUI::cmd(const char* fmt, ...) {
|
||||
char buf[80];
|
||||
char buf[96];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
@@ -37,6 +39,30 @@ void NextionUI::cmd(const char* fmt, ...) {
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
}
|
||||
|
||||
void NextionUI::put(const char* fmt, ...) {
|
||||
char buf[96];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
vsnprintf(buf, sizeof(buf), fmt, ap);
|
||||
va_end(ap);
|
||||
uint32_t h = 2166136261u; // FNV-1a
|
||||
for (const char* c = buf; *c; c++) h = (h ^ (uint8_t)*c) * 16777619u;
|
||||
h |= 1; // 0 = empty slot
|
||||
bool cached = slot_ < MAX_SLOTS;
|
||||
if (cached && !full_ && cache_[slot_] == h) { slot_++; return; }
|
||||
if (cached) cache_[slot_] = h;
|
||||
slot_++;
|
||||
NEXTION_SERIAL.print(buf);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
NEXTION_SERIAL.write(0xFF);
|
||||
}
|
||||
|
||||
void NextionUI::setPage(uint8_t page) {
|
||||
page_ = page;
|
||||
force_full_ = true; // a newly shown page has its HMI defaults: write everything
|
||||
}
|
||||
|
||||
// ---- Receive ----
|
||||
|
||||
bool NextionUI::decodeCustom(UiEvent& out) {
|
||||
@@ -55,26 +81,37 @@ bool NextionUI::decodeCustom(UiEvent& out) {
|
||||
case 'Z': out.cmd = UiCmd::ZeroGyro; break;
|
||||
case 'C': out.cmd = UiCmd::CalStartSave; break;
|
||||
case 'Q': out.cmd = UiCmd::CalCancel; break;
|
||||
case 'U': out.cmd = UiCmd::NextionUpload; break;
|
||||
case 'U': out.cmd = UiCmd::UploadRequest; break;
|
||||
case 'O': out.cmd = UiCmd::ServoCalToggle; break;
|
||||
case 'L': out.cmd = UiCmd::ServoCalLeg; break;
|
||||
case 'J': out.cmd = UiCmd::ServoCalJoint; break;
|
||||
case 'T': out.cmd = UiCmd::ServoCalTrim; break;
|
||||
case 'W': out.cmd = UiCmd::ServoCalSave; break;
|
||||
case 'E': out.cmd = UiCmd::ServoCalRevert; break;
|
||||
case 'P':
|
||||
if (out.value == page_) return false; // same page again: nothing changes
|
||||
setPage(out.value);
|
||||
out.cmd = UiCmd::PageShown;
|
||||
break;
|
||||
default: return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Standard Nextion frames (terminated FF FF FF). Two matter:
|
||||
// 0x88 / 0x00 display (re)started -> redraw everything
|
||||
// 0x65 page id event touch event from a component with "Send Component
|
||||
// ID" ticked - how the old HMI's upload button (b2)
|
||||
// reports, so it keeps working unchanged.
|
||||
// 0x88 / 0x00 display (re)started -> redraw everything (its start page
|
||||
// then reports itself with 'P')
|
||||
// 0x66 page reply to `sendme`: the page that is showing
|
||||
bool NextionUI::decodeNative(UiEvent& out) {
|
||||
if (native_buf_[0] == 0x88 || native_buf_[0] == 0x00) {
|
||||
force_full_ = true;
|
||||
return false;
|
||||
}
|
||||
if (native_buf_[0] == 0x65 && native_len_ >= 4 &&
|
||||
native_buf_[1] == NEXTION_UPLOAD_BUTTON_PAGE && native_buf_[2] == NEXTION_UPLOAD_BUTTON_ID) {
|
||||
out.cmd = UiCmd::NextionUpload;
|
||||
out.value = 0;
|
||||
if (native_buf_[0] == 0x66 && native_len_ >= 5) {
|
||||
if (native_buf_[1] == page_) return false;
|
||||
setPage(native_buf_[1]);
|
||||
out.cmd = UiCmd::PageShown;
|
||||
out.value = native_buf_[1];
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
@@ -131,62 +168,170 @@ bool NextionUI::poll(UiEvent& out) {
|
||||
|
||||
// ---- Render ----
|
||||
|
||||
void NextionUI::pushState(const UiView& v, bool full) {
|
||||
const UiView& s = shown_;
|
||||
if (full || v.armed != s.armed) {
|
||||
cmd(P "bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED");
|
||||
cmd(P "bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF);
|
||||
static const char* robotStateName(uint8_t s) {
|
||||
switch (s) {
|
||||
case TSTATE_DISARMED: return "DISARMED";
|
||||
case TSTATE_SIT: return "SIT";
|
||||
case TSTATE_STAND: return "STAND";
|
||||
case TSTATE_WALKING: return "WALKING";
|
||||
case TSTATE_SERVO_CAL: return "SERVO CAL";
|
||||
default: return "?";
|
||||
}
|
||||
if (full || v.stand != s.stand) {
|
||||
cmd(P "bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
|
||||
cmd(P "bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
|
||||
}
|
||||
if (full || v.gait_wave != s.gait_wave) cmd(P "bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
|
||||
if (full || v.mode_b != s.mode_b) cmd(P "bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
|
||||
if (full || v.rstick_xlate != s.rstick_xlate) cmd(P "bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
|
||||
if (full || v.remote_gyro != s.remote_gyro) cmd(P "bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
|
||||
if (full || v.robot_gyro != s.robot_gyro) cmd(P "bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
|
||||
if (full || v.speed_target != s.speed_target) {
|
||||
cmd(P "hSpeed.val=%u", v.speed_target);
|
||||
cmd(P "nSpeed.val=%u", (v.speed_target * 100 + 127) / 255);
|
||||
}
|
||||
if (full || v.height != s.height) {
|
||||
cmd(P "hHeight.val=%d", v.height / 10 + 100); // slider 0..200, 100 = default
|
||||
cmd(P "nHeight.val=%d", v.height / 10); // -100..100 %
|
||||
}
|
||||
if (full || v.calibrating != s.calibrating) {
|
||||
cmd(P "bCal.txt=\"%s\"", v.calibrating ? "SAVE CAL" : "CAL");
|
||||
cmd(P "bCal.bco=%u", v.calibrating ? COL_WARN : COL_OFF);
|
||||
}
|
||||
if (full || v.message != s.message) cmd(P "tMsg.txt=\"%s\"", v.message);
|
||||
}
|
||||
|
||||
void NextionUI::pushLink(const UiView& v) {
|
||||
if (v.link_ok) cmd(P "tLink.txt=\"LINK %u%%\"", v.link_pct);
|
||||
else cmd(P "tLink.txt=\"NO LINK\"");
|
||||
cmd(P "tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER);
|
||||
// The old HMI's own status texts, same wording as the old firmware.
|
||||
for (const char* t : NEXTION_STATUS_TEXTS)
|
||||
cmd("%s.txt=\"%s\"", t, v.link_ok ? "connected" : "not connected");
|
||||
// "7.2V", or "--" when the battery isn't measured.
|
||||
static const char* volts(char* buf, size_t n, uint16_t mv) {
|
||||
if (mv == 0) snprintf(buf, n, "--");
|
||||
else snprintf(buf, n, "%u.%uV", mv / 1000, (mv % 1000) / 100);
|
||||
return buf;
|
||||
}
|
||||
|
||||
// Angles are shown to 0.5 deg, so sensor noise doesn't repaint every frame.
|
||||
static float halfDeg(float d) { return roundf(d * 2.0f) / 2.0f; }
|
||||
|
||||
void NextionUI::renderTopBar(const UiView& v) {
|
||||
if (v.link_ok) put("tLink.txt=\"LINK %u%%\"", v.link_pct);
|
||||
else put("tLink.txt=\"NO LINK\"");
|
||||
put("tLink.pco=%u", v.link_ok ? COL_ON : COL_DANGER);
|
||||
|
||||
char b[8];
|
||||
if (v.tele_ok) put("tState.txt=\"ROBOT %s %s\"", robotStateName(v.tele.state), volts(b, sizeof(b), v.tele.battery_mv));
|
||||
else put("tState.txt=\"ROBOT: NO DATA\"");
|
||||
put("tState.pco=%u", v.tele_ok ? COL_TEXT : COL_DIM);
|
||||
}
|
||||
|
||||
void NextionUI::renderDrive(const UiView& v) {
|
||||
put("bArm.txt=\"%s\"", v.armed ? "ARMED" : "DISARMED");
|
||||
put("bArm.bco=%u", v.armed ? COL_DANGER : COL_OFF);
|
||||
put("bStand.txt=\"%s\"", v.stand ? "STAND" : "SIT");
|
||||
put("bStand.bco=%u", v.stand ? COL_ON : COL_OFF);
|
||||
put("bGait.txt=\"%s\"", v.gait_wave ? "WAVE" : "TRIPOD");
|
||||
put("bMode.txt=\"%s\"", v.mode_b ? "HEADING" : "DIRECT");
|
||||
put("bRs.txt=\"%s\"", v.rstick_xlate ? "R: MOVE" : "R: ROTATE");
|
||||
put("bRGyro.bco=%u", v.remote_gyro ? COL_ON : COL_OFF);
|
||||
put("bBGyro.bco=%u", v.robot_gyro ? COL_ON : COL_OFF);
|
||||
put("hSpeed.val=%u", v.speed_target);
|
||||
put("tSpeed.txt=\"%u%%\"", (v.speed_target * 100 + 127) / 255);
|
||||
put("hHeight.val=%d", v.height / 10 + 100); // slider 0..200, 100 = default
|
||||
put("tHeight.txt=\"%+d%%\"", v.height / 10);
|
||||
put("tMsg.txt=\"%s\"", v.message);
|
||||
}
|
||||
|
||||
void NextionUI::renderStatus(const UiView& v) {
|
||||
// Handset IMU, sliders 0..90 = -45..+45 deg
|
||||
if (v.imu_ok) {
|
||||
put("tRRoll.txt=\"roll %+.1f\"", halfDeg(v.imu_roll));
|
||||
put("tRPitch.txt=\"pitch %+.1f\"", halfDeg(v.imu_pitch));
|
||||
} else {
|
||||
put("tRRoll.txt=\"roll no IMU\"");
|
||||
put("tRPitch.txt=\"pitch no IMU\"");
|
||||
}
|
||||
put("hRRoll.val=%d", (int)constrain(lroundf(v.imu_roll) + 45, 0L, 90L));
|
||||
put("hRPitch.val=%d", (int)constrain(lroundf(v.imu_pitch) + 45, 0L, 90L));
|
||||
|
||||
// Robot IMU, from telemetry
|
||||
float br = v.tele.roll_ddeg / 10.0f, bp = v.tele.pitch_ddeg / 10.0f;
|
||||
bool robot_imu = v.tele_ok && (v.tele.status & TSTAT_IMU_OK);
|
||||
if (robot_imu) {
|
||||
put("tBRoll.txt=\"roll %+.1f\"", halfDeg(br));
|
||||
put("tBPitch.txt=\"pitch %+.1f\"", halfDeg(bp));
|
||||
} else {
|
||||
put("tBRoll.txt=\"roll %s\"", v.tele_ok ? "no IMU" : "--");
|
||||
put("tBPitch.txt=\"pitch %s\"", v.tele_ok ? "no IMU" : "--");
|
||||
}
|
||||
put("hBRoll.val=%d", robot_imu ? (int)constrain(lroundf(br) + 45, 0L, 90L) : 45);
|
||||
put("hBPitch.val=%d", robot_imu ? (int)constrain(lroundf(bp) + 45, 0L, 90L) : 45);
|
||||
|
||||
// Sticks, sliders 0..200 = -1000..+1000
|
||||
static const char* const STICKS[NUM_AXES] = { "hLX", "hLY", "hLT", "hRX", "hRY", "hRT" };
|
||||
for (int i = 0; i < NUM_AXES; i++) put("%s.val=%d", STICKS[i], (v.sticks[i] + 1000) / 10);
|
||||
|
||||
char a[8], b[8];
|
||||
put("tBatt.txt=\"battery remote %s robot %s\"", volts(a, sizeof(a), v.battery_mv),
|
||||
v.tele_ok ? volts(b, sizeof(b), v.tele.battery_mv) : "--");
|
||||
if (!v.link_ok) put("tRadio.txt=\"radio no link\"");
|
||||
else if (v.tele_ok) put("tRadio.txt=\"radio %u%% acked, robot hears %u/s\"", v.link_pct, v.tele.rx_per_sec);
|
||||
else put("tRadio.txt=\"radio %u%% acked, no telemetry\"", v.link_pct);
|
||||
}
|
||||
|
||||
void NextionUI::renderServos(const UiView& v) {
|
||||
for (int i = 0; i < 18; i++) {
|
||||
int joint = i % 3;
|
||||
if (v.tele_ok) {
|
||||
int deg = v.tele.joint_deg[i] - (joint == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0);
|
||||
// Bar = position within the servo's travel: coxa/femur -90..90, tibia -180..0.
|
||||
int pct = (deg + (joint == 2 ? 180 : 90)) * 100 / 180;
|
||||
put("tJ%d.txt=\"%-5s %+4d\"", i, JOINT_NAMES[joint], deg);
|
||||
put("jJ%d.val=%d", i, constrain(pct, 0, 100));
|
||||
} else {
|
||||
put("tJ%d.txt=\"%-5s --\"", i, JOINT_NAMES[joint]);
|
||||
put("jJ%d.val=0", i);
|
||||
}
|
||||
}
|
||||
const char* s = !v.tele_ok ? "NO DATA"
|
||||
: v.tele.state == TSTATE_SERVO_CAL ? "CENTRE"
|
||||
: (v.tele.status & TSTAT_SERVOS_ON) ? "LIVE"
|
||||
: "RELAXED";
|
||||
put("tSrv.txt=\"%s\"", s);
|
||||
put("tSrv.pco=%u", !v.tele_ok ? COL_DIM : (v.tele.status & TSTAT_SERVOS_ON) ? COL_ON : 0);
|
||||
}
|
||||
|
||||
void NextionUI::renderCal(const UiView& v) {
|
||||
put("bCalOn.txt=\"%s\"", v.servo_cal ? "STOP" : "START");
|
||||
put("bCalOn.bco=%u", v.servo_cal ? COL_WARN : COL_OFF);
|
||||
for (int i = 0; i < 6; i++) put("bL%d.bco=%u", i, v.servo_sel / 3 == i ? COL_ON : COL_OFF);
|
||||
for (int j = 0; j < 3; j++) put("bJ%d.bco=%u", j, v.servo_sel % 3 == j ? COL_ON : COL_OFF);
|
||||
|
||||
const char* leg = LEG_NAMES[v.servo_sel / 3];
|
||||
const char* joint = JOINT_NAMES[v.servo_sel % 3];
|
||||
if (!v.servo_cal) put("tTrim.txt=\"%s %s\"", leg, joint);
|
||||
else if (!v.trim_synced) put("tTrim.txt=\"%s %s ...\"", leg, joint);
|
||||
else put("tTrim.txt=\"%s %s %+dus\"", leg, joint, v.trim_us);
|
||||
|
||||
bool dirty = v.tele_ok && (v.tele.status & TSTAT_CAL_DIRTY);
|
||||
put("bSave.bco=%u", dirty ? COL_WARN : COL_OFF);
|
||||
|
||||
const char* hint = v.message[0] ? v.message
|
||||
: !v.servo_cal ? "Robot on a stand, disarmed: START"
|
||||
: !v.tele_ok ? "No telemetry from the robot"
|
||||
: v.tele.state != TSTATE_SERVO_CAL ? "Waiting for the robot"
|
||||
: dirty ? "Unsaved trims: SAVE keeps them"
|
||||
: "Trim until the horn is square";
|
||||
put("tMsg.txt=\"%s\"", hint);
|
||||
}
|
||||
|
||||
void NextionUI::renderSetup(const UiView& v) {
|
||||
put("bCal.txt=\"%s\"", v.stick_calibrating ? "SAVE CAL" : "CAL STICKS");
|
||||
put("bCal.bco=%u", v.stick_calibrating ? COL_WARN : COL_OFF);
|
||||
put("bUpd.txt=\"%s\"", v.upload_confirm ? "TAP AGAIN" : "UPDATE DISPLAY");
|
||||
put("bUpd.bco=%u", v.upload_confirm ? COL_WARN : COL_OFF);
|
||||
put("tVer.txt=\"remote fw %s protocol v%u radio ch %u\"", __DATE__, HXP_PROTOCOL_VERSION, HXP_RADIO_CHANNEL);
|
||||
put("tMsg.txt=\"%s\"", v.message);
|
||||
}
|
||||
|
||||
void NextionUI::render(const UiView& v, uint32_t now_ms) {
|
||||
bool full = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS;
|
||||
if (full) {
|
||||
uint32_t interval = page_ == PAGE_SERVOS ? NEXTION_SERVOS_RENDER_MS : NEXTION_RENDER_MS;
|
||||
if (!force_full_ && (now_ms - last_render_ms_) < interval) return;
|
||||
// Never block the TX loop on the display: wait for the buffer to drain.
|
||||
if (NEXTION_SERIAL.availableForWrite() < NEXTION_TX_MIN_FREE) return;
|
||||
last_render_ms_ = now_ms;
|
||||
|
||||
full_ = force_full_ || (now_ms - last_full_ms_) >= NEXTION_FULL_REFRESH_MS;
|
||||
if (full_) {
|
||||
force_full_ = false;
|
||||
last_full_ms_ = now_ms;
|
||||
if (page_ == PAGE_UNKNOWN) cmd("sendme"); // still don't know the page: ask again
|
||||
}
|
||||
slot_ = 0;
|
||||
|
||||
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;
|
||||
switch (page_) {
|
||||
case PAGE_DRIVE: renderTopBar(v); renderDrive(v); break;
|
||||
case PAGE_STATUS: renderTopBar(v); renderStatus(v); break;
|
||||
case PAGE_SERVOS: renderTopBar(v); renderServos(v); break;
|
||||
case PAGE_CAL: renderTopBar(v); renderCal(v); break;
|
||||
case PAGE_SETUP: renderTopBar(v); renderSetup(v); break;
|
||||
default: break; // splash / upload / unknown: nothing to show
|
||||
}
|
||||
|
||||
shown_ = v;
|
||||
}
|
||||
|
||||
// ---- Upload passthrough ----
|
||||
|
||||
@@ -7,17 +7,21 @@
|
||||
// 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: a
|
||||
// display reset (0x88 "ready") triggers a full redraw, and the old HMI's
|
||||
// upload button sends a standard touch event (0x65) that's recognized.
|
||||
// Command bytes are listed in NEXTION.md and in NextionUI.cpp. Every
|
||||
// page's Preinitialize event sends 'P' + its page id, so the remote always
|
||||
// knows which page is showing. Native Nextion return frames (terminated
|
||||
// FF FF FF) are parsed too: a display reset (0x88 "ready") triggers a
|
||||
// full redraw, and the reply to `sendme` (0x66 page) gives the page after
|
||||
// the remote itself restarted.
|
||||
//
|
||||
// remote -> Nextion: plain instructions ("bArm.txt=\"ARMED\"" + FF FF FF).
|
||||
//
|
||||
// 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.
|
||||
// Only the page that is showing is written (by local component names), and
|
||||
// only what changed: every instruction is hashed into a per-page cache. The
|
||||
// remote is authoritative: buttons are plain buttons, 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();
|
||||
@@ -35,14 +39,29 @@ private:
|
||||
uint8_t native_buf_[8];
|
||||
uint8_t native_len_ = 0, ff_run_ = 0;
|
||||
|
||||
UiView shown_;
|
||||
uint8_t page_ = PAGE_UNKNOWN;
|
||||
bool force_full_ = true;
|
||||
uint32_t last_full_ms_ = 0;
|
||||
uint32_t last_link_ms_ = 0;
|
||||
uint32_t last_render_ms_ = 0;
|
||||
|
||||
// Change cache: one hash per instruction, in the order a page's render
|
||||
// issues them (the same every time).
|
||||
static constexpr int MAX_SLOTS = 96;
|
||||
uint32_t cache_[MAX_SLOTS] = {};
|
||||
uint8_t slot_ = 0;
|
||||
bool full_ = false;
|
||||
|
||||
void cmd(const char* fmt, ...);
|
||||
void pushState(const UiView& v, bool full);
|
||||
void pushLink(const UiView& v);
|
||||
void put(const char* fmt, ...); // like cmd(), but only if it changed
|
||||
void setPage(uint8_t page);
|
||||
|
||||
void renderTopBar(const UiView& v);
|
||||
void renderDrive(const UiView& v);
|
||||
void renderStatus(const UiView& v);
|
||||
void renderServos(const UiView& v);
|
||||
void renderCal(const UiView& v);
|
||||
void renderSetup(const UiView& v);
|
||||
|
||||
bool decodeCustom(UiEvent& out);
|
||||
bool decodeNative(UiEvent& out);
|
||||
};
|
||||
@@ -14,6 +14,9 @@ CmdPacket PacketBuilder::build(const StickFrame& s, const RemoteIMU& imu, const
|
||||
p.seq = seq_++;
|
||||
p.speed_scale = st.speedCurrent();
|
||||
p.body_height = st.height(); // a setting, not motion: sent even while disarmed
|
||||
p.cal_cmd = st.servoCalCmd(now_ms); // CAL_NONE unless the CAL page is calibrating (only while disarmed)
|
||||
p.cal_servo = st.servoSel();
|
||||
p.cal_value = st.trimTarget();
|
||||
|
||||
uint8_t f = 0;
|
||||
if (st.armed()) f |= FLAG_ARMED;
|
||||
|
||||
@@ -1,29 +1,49 @@
|
||||
# Hexapod remote firmware (Teensy 4.1)
|
||||
# Hexapod remote firmware (Teensy 4.0)
|
||||
|
||||
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.
|
||||
at 100 Hz. The robot's `Telemetry` comes back in the radio's ack payloads.
|
||||
Without the robot, the Unity simulation can answer instead, through the
|
||||
USB nRF24 dongle (`../sim/README.md`).
|
||||
|
||||
## Planned
|
||||
|
||||
- **Battery level** (divider not built yet): read the 18650's voltage through a
|
||||
~100k/100k divider from the cell's + (not the boost board's 5 V output)
|
||||
into A6 (pin 20, free), with 100 nF to GND. Details in
|
||||
`../hardware/openscad/PLANNING.md` (notes for future designs). The
|
||||
firmware side is done (`Battery`, shown on the STATUS page): set
|
||||
`BATTERY_SENSE_PIN = A6` in `Config.h` once it's wired. A low-battery
|
||||
warning is still to do.
|
||||
|
||||
## Building
|
||||
|
||||
From the `2026-current` folder: `pio run -e remote`, flash with
|
||||
`pio run -e remote -t upload`, monitor with `pio device monitor -e remote`.
|
||||
The upload opens Teensy Loader; if the Teensy doesn't reboot into it by
|
||||
itself, press its program button. If flashing works but the USB serial port
|
||||
then won't open or goes quiet, try another USB port or cable before
|
||||
suspecting the firmware (a bare Teensy 4.0 showed exactly that on one port).
|
||||
|
||||
## Wiring (from the old remote sketch in `../../2019-teensy-nrf24/remote/`; edit `Config.h` if yours differs)
|
||||
## Wiring (edge headers only; edit `Config.h` if yours differs)
|
||||
|
||||
| Part | Signal | Teensy 4.1 pin |
|
||||
| Part | Signal | Teensy 4.0 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 | **25 / 24 (Wire2)**: not 18/19, those are stick pins A4/A5 |
|
||||
| Nextion | TX → RX1, RX ← TX1 | 0 / 1 (Serial1), 5 V supply |
|
||||
| Sticks | 6 axes | A0–A5. Which axis is on which pin isn't known yet: reorder `STICK_PINS` until the `p` dump reads right |
|
||||
| MPU6050 | SDA / SCL | **18 / 19 (Wire)** |
|
||||
| MPU6050 | VCC | 3.3 V (a GY-521 runs fine on it). Check the polarity: one supply reversal killed the first board's regulator |
|
||||
| Nextion | TX → RX2, RX ← TX2 | 7 / 8 (Serial2), 5 V supply (found on the real remote) |
|
||||
| Sticks | 6 axes | left stick X / Y / twist on **A3 / A7 / A8** (17 / 21 / 22), right stick on **A0 / A1 / A2** (14 / 15 / 16). A7/A8 hold the two wires that were on A4/A5 (18/19). All but the right twist read inverted (`STICK_INVERT`). Checked on the real remote |
|
||||
| Battery (planned) | divider | A6 (20) |
|
||||
|
||||
**MPU6050 rewiring:** the old remote had its MPU code switched off, probably
|
||||
because A4/A5 double as Wire's pins. Move the MPU's SDA/SCL to pins 25/24.
|
||||
**Rewiring from the old remote:** move the two stick wires from 18/19 to
|
||||
21/22, then put the MPU on 18/19 (Wire). The old remote had its MPU code
|
||||
switched off, probably because A4/A5 double as Wire's pins. Redo the stick
|
||||
axis check (`p` dump) and the sweep calibration afterwards.
|
||||
|
||||
Stick pots are wired 3.3 V – wiper – GND. **Never feed 5 V into a Teensy
|
||||
4.x analog pin.**
|
||||
@@ -46,14 +66,44 @@ steers: forward+left or back+left walk while turning left, and left alone
|
||||
turns on the spot for as long as you hold it. Let go and the robot holds
|
||||
its heading.
|
||||
|
||||
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.
|
||||
Touchscreen (`NEXTION.md` has the details), with tabs along the bottom and
|
||||
a red DISARM on every page:
|
||||
|
||||
- **DRIVE:** ARM / DISARM, SIT/STAND, TRIPOD/WAVE, DIRECT/HEADING,
|
||||
R: ROTATE/MOVE, REMOTE GYRO, ROBOT GYRO, speed and height sliders.
|
||||
- **STATUS:** handset and robot IMU roll/pitch, the six stick axes, both
|
||||
batteries, radio quality and the robot's received packet rate.
|
||||
- **SERVOS:** all 18 joint angles the robot is commanding, as a top view.
|
||||
- **CAL:** servo centre calibration (see below).
|
||||
- **SETUP:** stick calibration (CAL STICKS / CANCEL), LEVEL REMOTE (re-zero
|
||||
the handset tilt), UPDATE DISPLAY, backlight.
|
||||
|
||||
Everything about the robot comes from its **telemetry**, which rides back on
|
||||
the radio's ack payloads (`Telemetry` in `HexapodProtocol.h`): `RadioLink`
|
||||
reads it after every send, and it counts as stale after
|
||||
`TELEMETRY_STALE_MS`.
|
||||
|
||||
## Servo centre calibration (CAL page)
|
||||
|
||||
1. Put the robot on a stand, legs free, and disarm.
|
||||
2. Tap **START**. The robot drives every servo to its centre pose: coxa 0,
|
||||
femur 0, tibia −90 (legs straight out, shins straight down).
|
||||
3. Pick a leg (FR MR RR RL ML FL) and a joint. The remote first reads that
|
||||
servo's trim back from the robot (the trim box shows `...` until then).
|
||||
4. Step the trim with −10 / −1 / +1 / +10 µs (or ZERO) until the horn is
|
||||
exactly square. SAVE TO ROBOT turns orange while there are unsaved
|
||||
trims.
|
||||
5. **SAVE TO ROBOT** stores all 18 trims in the robot's EEPROM. **REVERT**
|
||||
drops unsaved changes. **STOP**, leaving the page, or a link loss relaxes
|
||||
the servos. Arming is refused while calibrating.
|
||||
|
||||
### Safety interlocks (in `RemoteState`)
|
||||
|
||||
- **Arming** is refused unless every stick axis is centered, and never
|
||||
allowed during calibration.
|
||||
allowed during stick or servo calibration. Servo calibration only starts
|
||||
while disarmed, and stops when you leave the CAL page or the link drops.
|
||||
- **UPDATE DISPLAY** needs a second tap within 3 s and is refused while
|
||||
armed.
|
||||
- **Disarming** always clears the stand request. The robot arms into SIT,
|
||||
so re-arming never stands it up by itself.
|
||||
- **Link loss:** if no ack arrives for `LINK_LOST_MS` (300 ms, matching the
|
||||
@@ -79,12 +129,13 @@ handset tilt), CAL / CANCEL CAL. `NEXTION.md` has the HMI build guide.
|
||||
|
||||
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.
|
||||
2. For full calibration, disarm, then tap **CAL STICKS** on the SETUP page
|
||||
(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, so it survives power cycles and reflashing.
|
||||
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.
|
||||
the save is refused and you stay in calibration mode. **CANCEL** (`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.
|
||||
|
||||
@@ -104,19 +155,31 @@ display attached.
|
||||
| `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 | `U` | Nextion upload mode (USB ↔ display; power-cycle to leave) |
|
||||
| `o` | servo calibration start/stop | `n` | next servo (leg × 3 + joint) |
|
||||
| `,` / `.` | trim −1 / +1 µs | `<` / `>` | trim −10 / +10 µs |
|
||||
| `0` | trim to zero | `w` / `e` | save trims on the robot / revert |
|
||||
|
||||
`p` also prints the robot's telemetry (state, IMU, battery, joint angles)
|
||||
and the servo-calibration state.
|
||||
|
||||
## Bench-check checklist
|
||||
|
||||
1. **Stick directions:** with `p`, confirm each logical axis reads
|
||||
positive for right / forward / counterclockwise twist. Flip it in
|
||||
`STICK_INVERT[]` if not.
|
||||
`STICK_INVERT[]` if not. Done on the real remote (2026-11-17), then
|
||||
the sweep calibration.
|
||||
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.
|
||||
`IMU_AXIS_SIGN`. Done (defaults are right). The first GY-521 got its
|
||||
supply reversed once and its regulator now runs hot: redo this check
|
||||
on the replacement, and see whether it still needs the reset in
|
||||
`RemoteIMU::begin()`.
|
||||
3. **Nextion:** the generated HMI sets `bauds=115200` in Program.s and is
|
||||
on the display (upload through SETUP › UPDATE DISPLAY or serial `U`).
|
||||
4. **Radio:** with the robot (or the Unity sim behind the USB dongle)
|
||||
running, `tLink` / `p` should show ~100%, and `p` should print robot
|
||||
telemetry. If it reads NO LINK, check that both ends were built from
|
||||
the same `HexapodProtocol.h`: each end drops packets of the wrong size.
|
||||
Done against the Unity sim through the dongle; not yet with the robot.
|
||||
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.
|
||||
@@ -11,7 +11,10 @@ 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)
|
||||
// The robot answers with Telemetry in its ack payloads, which needs
|
||||
// dynamic payload lengths on both radios. Each end checks the length of
|
||||
// what it receives against its own struct instead (see HexapodProtocol.h).
|
||||
radio.enableAckPayload();
|
||||
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
|
||||
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
|
||||
radio.setChannel(HXP_RADIO_CHANNEL);
|
||||
@@ -30,6 +33,18 @@ bool RadioLink::send(const CmdPacket& p) {
|
||||
last_ack_ms_ = millis();
|
||||
ever_acked_ = true;
|
||||
}
|
||||
// An ack payload lands in the RX FIFO. Drain it every time, so it can't fill up.
|
||||
while (radio.available()) {
|
||||
uint8_t len = radio.getDynamicPayloadSize();
|
||||
if (len == sizeof(Telemetry)) {
|
||||
radio.read(&telemetry_, sizeof(telemetry_));
|
||||
has_telemetry_ = true;
|
||||
telemetry_ms_ = millis();
|
||||
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
|
||||
uint8_t junk[32]; // another protocol version: ignore it
|
||||
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
|
||||
}
|
||||
}
|
||||
return acked;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
#pragma once
|
||||
#include <HexapodProtocol.h>
|
||||
#include "Config.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.
|
||||
// The robot's Telemetry comes back as the ack's payload.
|
||||
class RadioLink {
|
||||
public:
|
||||
bool begin(); // false if the radio chip didn't respond
|
||||
@@ -13,10 +15,19 @@ public:
|
||||
bool linkOk() const { return link_ok_; }
|
||||
uint8_t qualityPct() const; // acked share of the last 32 packets
|
||||
|
||||
// Latest telemetry from the robot, and whether it's recent enough to show.
|
||||
const Telemetry& telemetry() const { return telemetry_; }
|
||||
bool telemetryFresh(uint32_t now_ms) const {
|
||||
return has_telemetry_ && (now_ms - telemetry_ms_) < TELEMETRY_STALE_MS;
|
||||
}
|
||||
|
||||
private:
|
||||
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
|
||||
Telemetry telemetry_;
|
||||
bool has_telemetry_ = false;
|
||||
uint32_t telemetry_ms_ = 0;
|
||||
};
|
||||
@@ -18,8 +18,15 @@ static bool writeReg(uint8_t reg, uint8_t val) {
|
||||
bool RemoteIMU::begin() {
|
||||
IMU_WIRE.begin();
|
||||
IMU_WIRE.setClock(400000);
|
||||
ok_ = writeReg(REG_PWR_MGMT_1, 0x00); // wake out of sleep
|
||||
// Full device reset first: the remote's MPU6050 (likely a clone) answers
|
||||
// and wakes without it, but every data register reads 0 until it has been
|
||||
// reset (found on the real remote, 2026-11-17).
|
||||
ok_ = writeReg(REG_PWR_MGMT_1, 0x80);
|
||||
if (!ok_) return false;
|
||||
delay(100);
|
||||
ok_ = writeReg(REG_PWR_MGMT_1, 0x01); // wake, clocked from the X gyro PLL
|
||||
if (!ok_) return false;
|
||||
delay(50);
|
||||
writeReg(REG_CONFIG, 0x03); // 44 Hz DLPF: a handheld remote doesn't need more bandwidth
|
||||
delay(50);
|
||||
|
||||
|
||||
@@ -17,6 +17,26 @@ void RemoteState::disarm() {
|
||||
stand_ = false;
|
||||
}
|
||||
|
||||
void RemoteState::stopServoCal() {
|
||||
servo_cal_ = false;
|
||||
trim_synced_ = false;
|
||||
cal_op_ = CAL_NONE;
|
||||
}
|
||||
|
||||
// A new selection is only trimmed once the robot has reported that servo's
|
||||
// current trim (CAL_HOLD until then), so a stale value is never sent.
|
||||
void RemoteState::selectServo(uint8_t servo) {
|
||||
if (servo == servo_sel_) return;
|
||||
servo_sel_ = servo;
|
||||
trim_synced_ = false;
|
||||
}
|
||||
|
||||
uint8_t RemoteState::servoCalCmd(uint32_t now_ms) const {
|
||||
if (!servo_cal_ || armed_) return CAL_NONE;
|
||||
if (cal_op_ != CAL_NONE && now_ms < cal_op_until_ms_) return cal_op_;
|
||||
return trim_synced_ ? CAL_SET : CAL_HOLD;
|
||||
}
|
||||
|
||||
bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uint32_t now) {
|
||||
const StickFrame& sticks = stick_hw.frame();
|
||||
switch (e.cmd) {
|
||||
@@ -30,6 +50,7 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
|
||||
case UiCmd::ToggleArm:
|
||||
if (armed_) { disarm(); return true; }
|
||||
if (calibrating_) { setMessage("Finish calibration first", now); return false; }
|
||||
if (servo_cal_) { setMessage("Stop servo calibration first", now); return false; }
|
||||
if (!sticks.allCentered(ARM_CENTER_TOLERANCE)) { setMessage("Center both sticks to arm", now); return false; }
|
||||
armed_ = true;
|
||||
return true;
|
||||
@@ -81,6 +102,66 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
|
||||
case UiCmd::NextionUpload:
|
||||
return false; // not operator state - remote.ino handles it
|
||||
|
||||
case UiCmd::UploadRequest:
|
||||
if (armed_) { setMessage("Disarm first", now); return false; }
|
||||
if (now < upload_confirm_until_ms_) { upload_go_ = true; return true; }
|
||||
upload_confirm_until_ms_ = now + UPLOAD_CONFIRM_MS;
|
||||
setMessage("Tap again to enter display update mode", now);
|
||||
return true;
|
||||
|
||||
case UiCmd::PageShown:
|
||||
page_ = e.value;
|
||||
// Servo calibration only runs while its page is showing.
|
||||
if (servo_cal_ && page_ != PAGE_CAL) stopServoCal();
|
||||
upload_confirm_until_ms_ = 0;
|
||||
return true;
|
||||
|
||||
case UiCmd::ServoCalToggle:
|
||||
if (servo_cal_) { stopServoCal(); setMessage("Servos relaxed", now); return true; }
|
||||
if (armed_) { setMessage("Disarm to calibrate servos", now); return false; }
|
||||
servo_cal_ = true;
|
||||
trim_synced_ = false;
|
||||
setMessage("Robot holds every servo at centre", now);
|
||||
return true;
|
||||
|
||||
case UiCmd::ServoCalLeg:
|
||||
if (e.value >= 6) return false;
|
||||
selectServo(e.value * 3 + servo_sel_ % 3);
|
||||
return true;
|
||||
|
||||
case UiCmd::ServoCalJoint:
|
||||
if (e.value >= 3) return false;
|
||||
selectServo(servo_sel_ / 3 * 3 + e.value);
|
||||
return true;
|
||||
|
||||
case UiCmd::ServoCalNext:
|
||||
selectServo((servo_sel_ + 1) % 18);
|
||||
return true;
|
||||
|
||||
case UiCmd::ServoCalTrim: {
|
||||
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
|
||||
if (!trim_synced_) { setMessage("Waiting for the robot", now); return false; }
|
||||
int t = trim_target_;
|
||||
switch (e.value) {
|
||||
case SERVO_TRIM_MINUS_BIG: t -= SERVO_TRIM_STEP_BIG_US; break;
|
||||
case SERVO_TRIM_MINUS_SMALL: t -= SERVO_TRIM_STEP_SMALL_US; break;
|
||||
case SERVO_TRIM_PLUS_SMALL: t += SERVO_TRIM_STEP_SMALL_US; break;
|
||||
case SERVO_TRIM_PLUS_BIG: t += SERVO_TRIM_STEP_BIG_US; break;
|
||||
case SERVO_TRIM_ZERO: t = 0; break;
|
||||
default: return false;
|
||||
}
|
||||
trim_target_ = (int16_t)constrain(t, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
|
||||
return true;
|
||||
}
|
||||
|
||||
case UiCmd::ServoCalSave:
|
||||
case UiCmd::ServoCalRevert:
|
||||
if (!servo_cal_) { setMessage("Tap START first", now); return false; }
|
||||
cal_op_ = e.cmd == UiCmd::ServoCalSave ? CAL_SAVE : CAL_REVERT;
|
||||
cal_op_until_ms_ = now + SERVO_CAL_OP_MS;
|
||||
setMessage(e.cmd == UiCmd::ServoCalSave ? "Trims saved on the robot" : "Unsaved trims dropped", now);
|
||||
return true;
|
||||
|
||||
case UiCmd::CalCancel:
|
||||
if (!calibrating_) return false;
|
||||
stick_hw.cancelCalibration();
|
||||
@@ -91,16 +172,35 @@ bool RemoteState::handle(const UiEvent& e, Sticks& stick_hw, RemoteIMU& imu, uin
|
||||
return false;
|
||||
}
|
||||
|
||||
void RemoteState::update(float dt, bool link_ok, uint32_t now_ms) {
|
||||
void RemoteState::update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms) {
|
||||
// Mirror the robot's link failsafe: it disarms (servos relax) when packets
|
||||
// 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);
|
||||
// once the link is back. Servo calibration stops the same way, so the
|
||||
// servos don't spring back to centre on their own when the link returns.
|
||||
if (!link_ok && link_was_ok_) {
|
||||
if (armed_) {
|
||||
disarm();
|
||||
setMessage("Link lost - disarmed", now_ms);
|
||||
}
|
||||
if (servo_cal_) {
|
||||
stopServoCal();
|
||||
setMessage("Link lost - servo cal stopped", now_ms);
|
||||
}
|
||||
}
|
||||
link_was_ok_ = link_ok;
|
||||
|
||||
// SAVE / REVERT done: after a revert the robot's trims changed under us, so
|
||||
// read the selected one back again.
|
||||
if (cal_op_ != CAL_NONE && now_ms >= cal_op_until_ms_) {
|
||||
if (cal_op_ == CAL_REVERT) trim_synced_ = false;
|
||||
cal_op_ = CAL_NONE;
|
||||
}
|
||||
if (servo_cal_ && !trim_synced_ && cal_op_ == CAL_NONE && tele_fresh &&
|
||||
tele.state == TSTATE_SERVO_CAL && tele.cal_servo == servo_sel_) {
|
||||
trim_target_ = tele.cal_trim_us;
|
||||
trim_synced_ = true;
|
||||
}
|
||||
|
||||
speed_.setTarget(speed_target_);
|
||||
speed_.update(dt);
|
||||
tilt_blend_.setTarget(remote_gyro_ ? 1.0f : 0.0f);
|
||||
@@ -115,7 +215,12 @@ void RemoteState::fillView(UiView& v, uint32_t now_ms) const {
|
||||
v.rstick_xlate = rstick_xlate_;
|
||||
v.remote_gyro = remote_gyro_;
|
||||
v.robot_gyro = robot_gyro_;
|
||||
v.calibrating = calibrating_;
|
||||
v.stick_calibrating = calibrating_;
|
||||
v.servo_cal = servo_cal_;
|
||||
v.servo_sel = servo_sel_;
|
||||
v.trim_synced = trim_synced_;
|
||||
v.trim_us = trim_target_;
|
||||
v.upload_confirm = now_ms < upload_confirm_until_ms_;
|
||||
v.speed_target = speed_target_;
|
||||
v.height = height_;
|
||||
v.message = message(now_ms);
|
||||
|
||||
@@ -17,8 +17,9 @@ public:
|
||||
// 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);
|
||||
// Advances smoothers and link-loss handling, and reads the servo trim back
|
||||
// from the robot's telemetry. Call once per TX tick.
|
||||
void update(float dt, bool link_ok, bool tele_fresh, const Telemetry& tele, uint32_t now_ms);
|
||||
|
||||
bool armed() const { return armed_; }
|
||||
bool standRequested() const { return stand_; }
|
||||
@@ -33,6 +34,17 @@ public:
|
||||
uint8_t speedCurrent() const { return (uint8_t)lroundf(speed_.current); }
|
||||
float remoteTiltBlend() const { return tilt_blend_.current; } // 0..1, fades on the gyro toggle
|
||||
|
||||
// Servo centre calibration (CAL page) -> CmdPacket.cal_*
|
||||
uint8_t servoCalCmd(uint32_t now_ms) const;
|
||||
uint8_t servoSel() const { return servo_sel_; } // leg * 3 + joint
|
||||
int16_t trimTarget() const { return trim_target_; }
|
||||
bool servoCal() const { return servo_cal_; }
|
||||
bool trimSynced() const { return trim_synced_; }
|
||||
|
||||
uint8_t page() const { return page_; }
|
||||
// UPDATE DISPLAY was confirmed: remote.ino enters the upload passthrough.
|
||||
bool takeUploadRequest() { bool r = upload_go_; upload_go_ = false; return r; }
|
||||
|
||||
const char* message(uint32_t now_ms) const { return now_ms < message_until_ms_ ? message_ : ""; }
|
||||
void setMessage(const char* msg, uint32_t now_ms);
|
||||
|
||||
@@ -53,9 +65,23 @@ private:
|
||||
SmoothedValue speed_{SPEED_SCALE_RATE_PER_SEC};
|
||||
SmoothedValue tilt_blend_{REMOTE_TILT_FADE_PER_SEC};
|
||||
|
||||
uint8_t page_ = PAGE_UNKNOWN;
|
||||
|
||||
bool servo_cal_ = false;
|
||||
uint8_t servo_sel_ = 0;
|
||||
int16_t trim_target_ = 0;
|
||||
bool trim_synced_ = false; // trim_target_ was read back from the robot for servo_sel_
|
||||
uint8_t cal_op_ = CAL_NONE; // CAL_SAVE / CAL_REVERT while it's being sent
|
||||
uint32_t cal_op_until_ms_ = 0;
|
||||
|
||||
uint32_t upload_confirm_until_ms_ = 0;
|
||||
bool upload_go_ = false;
|
||||
|
||||
bool link_was_ok_ = false;
|
||||
const char* message_ = "";
|
||||
uint32_t message_until_ms_ = 0;
|
||||
|
||||
void disarm();
|
||||
void stopServoCal();
|
||||
void selectServo(uint8_t servo);
|
||||
};
|
||||
@@ -23,6 +23,15 @@ bool SerialConsole::poll(UiEvent& ui, Local& local) {
|
||||
case 'c': ui.cmd = UiCmd::CalStartSave; return true;
|
||||
case 'x': ui.cmd = UiCmd::CalCancel; return true;
|
||||
case 'U': ui.cmd = UiCmd::NextionUpload; return true;
|
||||
case 'o': ui.cmd = UiCmd::ServoCalToggle; return true;
|
||||
case 'n': ui.cmd = UiCmd::ServoCalNext; return true;
|
||||
case ',': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_MINUS_SMALL }; return true;
|
||||
case '.': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_PLUS_SMALL }; return true;
|
||||
case '<': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_MINUS_BIG }; return true;
|
||||
case '>': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_PLUS_BIG }; return true;
|
||||
case '0': ui = UiEvent{ UiCmd::ServoCalTrim, SERVO_TRIM_ZERO }; return true;
|
||||
case 'w': ui.cmd = UiCmd::ServoCalSave; return true;
|
||||
case 'e': ui.cmd = UiCmd::ServoCalRevert; return true;
|
||||
case 'p': local = Local::PrintStatus; return true;
|
||||
case 'd': local = Local::ToggleDump; return true;
|
||||
case 'R': local = Local::ResetCal; return true;
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <HexapodProtocol.h>
|
||||
#include "Config.h"
|
||||
|
||||
// Operator commands. Produced by the Nextion touchscreen (NextionUI) and by
|
||||
// the USB serial console (SerialConsole) through the same path, so the
|
||||
@@ -21,19 +23,43 @@ enum class UiCmd : uint8_t {
|
||||
HeightUp, // serial console only
|
||||
HeightDown, // serial console only
|
||||
ZeroGyro, // capture the handset's current attitude as "level"
|
||||
CalStartSave, // start sweep calibration, or save it if running
|
||||
CalStartSave, // stick sweep calibration: start, or save it if running
|
||||
CalCancel,
|
||||
NextionUpload, // USB <-> display passthrough for Nextion Editor (handled in remote.ino)
|
||||
PageShown, // value = Nextion page id (UiPage); the display just showed it
|
||||
ServoCalToggle, // servo centre calibration on/off (robot holds the centre pose)
|
||||
ServoCalLeg, // value = leg 0..5
|
||||
ServoCalJoint, // value = joint 0..2 (coxa, femur, tibia)
|
||||
ServoCalNext, // serial console only: next servo
|
||||
ServoCalTrim, // value = SERVO_TRIM_* step code
|
||||
ServoCalSave, // store the trims in the robot's EEPROM
|
||||
ServoCalRevert, // reload the trims from the robot's EEPROM
|
||||
UploadRequest, // display update mode from the touchscreen: needs a second tap
|
||||
NextionUpload, // display update mode right away (serial console 'U')
|
||||
};
|
||||
|
||||
// ServoCalTrim values.
|
||||
constexpr uint8_t SERVO_TRIM_MINUS_BIG = 0;
|
||||
constexpr uint8_t SERVO_TRIM_MINUS_SMALL = 1;
|
||||
constexpr uint8_t SERVO_TRIM_PLUS_SMALL = 2;
|
||||
constexpr uint8_t SERVO_TRIM_PLUS_BIG = 3;
|
||||
constexpr uint8_t SERVO_TRIM_ZERO = 4;
|
||||
|
||||
struct UiEvent {
|
||||
UiCmd cmd = UiCmd::None;
|
||||
uint8_t value = 0;
|
||||
};
|
||||
|
||||
// The HMI's pages (remote/remote.HMI, page id = position in the Editor's
|
||||
// page list). Each page reports itself with a 'P' frame when it opens.
|
||||
enum UiPage : uint8_t {
|
||||
PAGE_SPLASH = 0, PAGE_DRIVE, PAGE_STATUS, PAGE_SERVOS, PAGE_CAL, PAGE_SETUP, PAGE_UPLOAD,
|
||||
PAGE_UNKNOWN = 0xFF,
|
||||
};
|
||||
|
||||
// Everything the display shows. The remote is authoritative: the Nextion
|
||||
// only sends requests, and always renders what's in here.
|
||||
struct UiView {
|
||||
// Operator state (DRIVE page)
|
||||
bool armed = false;
|
||||
bool stand = false;
|
||||
bool gait_wave = false;
|
||||
@@ -41,10 +67,30 @@ struct UiView {
|
||||
bool rstick_xlate = false;
|
||||
bool remote_gyro = false;
|
||||
bool robot_gyro = false;
|
||||
bool calibrating = false;
|
||||
uint8_t speed_target = 0;
|
||||
int16_t height = 0; // -1000..1000, 0 = default
|
||||
|
||||
// Radio + robot telemetry
|
||||
bool link_ok = false;
|
||||
uint8_t link_pct = 0;
|
||||
const char* message = ""; // always a string literal - NextionUI diffs it by pointer
|
||||
bool tele_ok = false; // fresh telemetry from the robot
|
||||
Telemetry tele;
|
||||
|
||||
// Handset (STATUS page)
|
||||
bool imu_ok = false;
|
||||
float imu_roll = 0, imu_pitch = 0; // degrees, vs the zeroed level
|
||||
int16_t sticks[NUM_AXES] = {};
|
||||
uint16_t battery_mv = 0; // 0 = not measured
|
||||
|
||||
// SETUP page
|
||||
bool stick_calibrating = false;
|
||||
bool upload_confirm = false; // UPDATE DISPLAY tapped once, waiting for the second tap
|
||||
|
||||
// CAL page (servo centre calibration)
|
||||
bool servo_cal = false;
|
||||
uint8_t servo_sel = 0; // leg * 3 + joint
|
||||
bool trim_synced = false; // trim below is known (read back from the robot)
|
||||
int16_t trim_us = 0;
|
||||
|
||||
const char* message = "";
|
||||
};
|
||||
Binary file not shown.
@@ -6,8 +6,9 @@
|
||||
#include "RadioLink.h"
|
||||
#include "NextionUI.h"
|
||||
#include "SerialConsole.h"
|
||||
#include "Battery.h"
|
||||
|
||||
// Hexapod remote (Teensy 4.1): reads two 3-axis sticks and the handset
|
||||
// Hexapod remote (Teensy 4.0): reads two 3-axis sticks and the handset
|
||||
// MPU6050, takes operator commands from the Nextion touchscreen (and the USB
|
||||
// serial console), and transmits a CmdPacket to the robot at TX_HZ.
|
||||
// See README.md for wiring, calibration and the serial key map.
|
||||
@@ -19,6 +20,7 @@ PacketBuilder builder;
|
||||
RadioLink radio_link;
|
||||
NextionUI nextion;
|
||||
SerialConsole console;
|
||||
Battery battery;
|
||||
|
||||
uint32_t last_tx_us = 0;
|
||||
bool dump_packets = false;
|
||||
@@ -39,6 +41,22 @@ static void printStatus() {
|
||||
state.modeB() ? "B" : "A", state.rstickXlate() ? "xlate" : "rot",
|
||||
state.remoteGyro(), state.robotGyro(), state.speedTarget(), state.speedCurrent(), state.height());
|
||||
sticks.printCalibration(Serial);
|
||||
if (BATTERY_SENSE_PIN != 255) Serial.printf("battery %u mV\n", battery.millivolts());
|
||||
|
||||
const Telemetry& t = radio_link.telemetry();
|
||||
if (radio_link.telemetryFresh(millis())) {
|
||||
Serial.printf("robot: state %u status %02X roll %.1f pitch %.1f battery %u mV, hears %u/s\n",
|
||||
t.state, t.status, t.roll_ddeg / 10.0f, t.pitch_ddeg / 10.0f, t.battery_mv, t.rx_per_sec);
|
||||
Serial.print("robot joints (deg):");
|
||||
for (int i = 0; i < 18; i++)
|
||||
Serial.printf(" %d", t.joint_deg[i] - (i % 3 == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0));
|
||||
Serial.println();
|
||||
} else {
|
||||
Serial.println("robot: no telemetry");
|
||||
}
|
||||
Serial.printf("servo cal %s, servo %u (leg %u joint %u) trim %d us%s, robot reports servo %u trim %d\n",
|
||||
state.servoCal() ? "ON" : "off", state.servoSel(), state.servoSel() / 3, state.servoSel() % 3,
|
||||
state.trimTarget(), state.trimSynced() ? "" : " (not read back yet)", t.cal_servo, t.cal_trim_us);
|
||||
}
|
||||
|
||||
static void dumpPacket(const CmdPacket& p) {
|
||||
@@ -47,7 +65,7 @@ static void dumpPacket(const CmdPacket& p) {
|
||||
p.remote_roll, p.remote_pitch, p.body_height, p.speed_scale, p.flags);
|
||||
}
|
||||
|
||||
// Nextion Editor upload through this USB port (the old remote's b2 button).
|
||||
// Nextion Editor upload through this USB port (SETUP > UPDATE DISPLAY, or 'U').
|
||||
// Never returns: the TX loop stops, so the robot loses the link and
|
||||
// disarms itself after LINK_TIMEOUT_SEC. Power-cycle the remote to leave.
|
||||
static void enterNextionUpload() {
|
||||
@@ -61,8 +79,8 @@ static void enterNextionUpload() {
|
||||
static void pollInputs(uint32_t now_ms) {
|
||||
UiEvent ev;
|
||||
while (nextion.poll(ev)) {
|
||||
if (ev.cmd == UiCmd::NextionUpload) enterNextionUpload();
|
||||
state.handle(ev, sticks, imu, now_ms);
|
||||
if (state.takeUploadRequest()) enterNextionUpload(); // UPDATE DISPLAY, tapped twice
|
||||
}
|
||||
|
||||
SerialConsole::Local local;
|
||||
@@ -114,7 +132,9 @@ void loop() {
|
||||
sticks.update();
|
||||
imu.update(dt);
|
||||
radio_link.update(now_ms);
|
||||
state.update(dt, radio_link.linkOk(), now_ms);
|
||||
battery.update(dt);
|
||||
bool tele_fresh = radio_link.telemetryFresh(now_ms);
|
||||
state.update(dt, radio_link.linkOk(), tele_fresh, radio_link.telemetry(), now_ms);
|
||||
|
||||
last_packet = builder.build(sticks.frame(), imu, state, now_ms);
|
||||
radio_link.send(last_packet);
|
||||
@@ -123,6 +143,13 @@ void loop() {
|
||||
state.fillView(view, now_ms);
|
||||
view.link_ok = radio_link.linkOk();
|
||||
view.link_pct = radio_link.qualityPct();
|
||||
view.tele_ok = tele_fresh;
|
||||
view.tele = radio_link.telemetry();
|
||||
view.imu_ok = imu.ok();
|
||||
view.imu_roll = imu.rollDeg();
|
||||
view.imu_pitch = imu.pitchDeg();
|
||||
for (int i = 0; i < NUM_AXES; i++) view.sticks[i] = sticks.frame().v[i];
|
||||
view.battery_mv = battery.millivolts();
|
||||
nextion.render(view, now_ms);
|
||||
|
||||
if (dump_packets && now_ms - last_dump_ms >= 100) {
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
#include "Battery.h"
|
||||
#include "Config.h"
|
||||
|
||||
void Battery::update(float dt) {
|
||||
if (BATTERY_SENSE_PIN == 255) return; // not fitted: stays 0
|
||||
float v = analogRead(BATTERY_SENSE_PIN) * (BATTERY_ADC_REF_V / BATTERY_ADC_MAX) * BATTERY_DIVIDER;
|
||||
if (!primed_) { volts_ = v; primed_ = true; return; }
|
||||
float a = constrain(dt / 2.0f, 0.0f, 1.0f); // ~2 s time constant: servo load makes it sag and bounce
|
||||
volts_ += (v - volts_) * a;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
// Battery voltage through a resistor divider (Config.h BATTERY_*), smoothed.
|
||||
class Battery {
|
||||
public:
|
||||
void update(float dt);
|
||||
uint16_t millivolts() const { return (uint16_t)(volts_ * 1000.0f + 0.5f); } // 0 = not fitted
|
||||
|
||||
private:
|
||||
float volts_ = 0;
|
||||
bool primed_ = false;
|
||||
};
|
||||
@@ -12,22 +12,48 @@ bool Comms::begin() {
|
||||
SPI.setSCK(RADIO_SCK_PIN); // alternate SCK pin (see Config.h); before RF24 starts SPI
|
||||
radio_present_ = radio.begin();
|
||||
if (radio_present_) {
|
||||
radio.setPayloadSize(HXP_RADIO_PAYLOAD); // mismatched protocol versions won't be received at all
|
||||
// Telemetry rides back on the auto-ack (ack payloads), which needs dynamic
|
||||
// payload lengths - update() drops any packet that isn't a CmdPacket.
|
||||
radio.enableAckPayload();
|
||||
radio.openReadingPipe(1, HXP_RADIO_ADDRESS);
|
||||
radio.setPALevel((rf24_pa_dbm_e)HXP_RADIO_PA_LEVEL);
|
||||
radio.setDataRate((rf24_datarate_e)HXP_RADIO_DATA_RATE);
|
||||
radio.setChannel(HXP_RADIO_CHANNEL);
|
||||
radio.startListening();
|
||||
loadAckPayload();
|
||||
}
|
||||
return radio_present_;
|
||||
}
|
||||
|
||||
// Replaces whatever ack payload is still queued with the latest telemetry, so
|
||||
// the remote never gets stale data.
|
||||
void Comms::loadAckPayload() {
|
||||
radio.flush_tx();
|
||||
radio.writeAckPayload(1, &telemetry_, sizeof(telemetry_));
|
||||
}
|
||||
|
||||
void Comms::update() {
|
||||
if (radio_present_) {
|
||||
bool got = false;
|
||||
while (radio.available()) {
|
||||
radio.read(&packet_, sizeof(packet_));
|
||||
last_rx_ms_ = millis();
|
||||
uint8_t len = radio.getDynamicPayloadSize();
|
||||
if (len == sizeof(CmdPacket)) {
|
||||
radio.read(&packet_, sizeof(packet_));
|
||||
last_rx_ms_ = millis();
|
||||
rx_count_++;
|
||||
got = true;
|
||||
} else if (len > 0) { // 0 = corrupt length; RF24 has flushed the FIFO already
|
||||
uint8_t junk[32]; // another protocol version: never act on it
|
||||
radio.read(junk, len <= sizeof(junk) ? len : sizeof(junk));
|
||||
}
|
||||
}
|
||||
if (got) loadAckPayload();
|
||||
}
|
||||
uint32_t now = millis();
|
||||
if (now - rate_window_ms_ >= 1000) {
|
||||
rx_per_sec_ = (uint8_t)min(rx_count_, (uint16_t)255);
|
||||
rx_count_ = 0;
|
||||
rate_window_ms_ = now;
|
||||
}
|
||||
link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000);
|
||||
}
|
||||
@@ -12,9 +12,20 @@ public:
|
||||
// Lets the serial test harness (or, later, real radio) inject a packet directly.
|
||||
void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); }
|
||||
|
||||
// Telemetry for the remote. Goes out as the ack payload of the next packet
|
||||
// received, so set it every loop.
|
||||
void setTelemetry(const Telemetry& t) { telemetry_ = t; }
|
||||
uint8_t packetsPerSec() const { return rx_per_sec_; }
|
||||
|
||||
private:
|
||||
CmdPacket packet_;
|
||||
uint32_t last_rx_ms_ = 0;
|
||||
bool link_ok_ = false;
|
||||
bool radio_present_ = false;
|
||||
Telemetry telemetry_;
|
||||
uint16_t rx_count_ = 0;
|
||||
uint8_t rx_per_sec_ = 0;
|
||||
uint32_t rate_window_ms_ = 0;
|
||||
|
||||
void loadAckPayload();
|
||||
};
|
||||
@@ -140,6 +140,15 @@ constexpr uint8_t RADIO_CSN_PIN = 15;
|
||||
constexpr uint8_t RADIO_SCK_PIN = 14;
|
||||
constexpr float LINK_TIMEOUT_SEC = 0.3f;
|
||||
|
||||
// ---- Battery voltage (shown on the remote's STATUS page) ----
|
||||
// Battery + through a resistor divider into an analog pin. BENCH: no divider
|
||||
// is wired yet - set the pin and the divider ratio (battery V / pin V) once
|
||||
// it is. 255 = not fitted: the remote then shows "--".
|
||||
constexpr uint8_t BATTERY_SENSE_PIN = 255;
|
||||
constexpr float BATTERY_DIVIDER = 3.0f; // e.g. 20k over 10k
|
||||
constexpr float BATTERY_ADC_REF_V = 3.3f; // Teensy 3.6 analog reference
|
||||
constexpr int BATTERY_ADC_MAX = 1023; // default 10-bit analogRead()
|
||||
|
||||
// ---- Robot-gyro self-leveling ----
|
||||
// Integral gain (1/s): a leftover tilt of 1 deg changes the correction by
|
||||
// this many deg/s. Higher levels faster but can wobble. BENCH: tune on the robot.
|
||||
|
||||
@@ -41,8 +41,9 @@ still a placeholder. Check all of it against the real robot:
|
||||
needed ±90° offsets there, which left the servos at the end of their
|
||||
travel.
|
||||
- `RADIO_CE_PIN` / `RADIO_CSN_PIN` / `RADIO_SCK_PIN`: 39 / 15 / 14, as in
|
||||
the old sketch. Address, channel, data rate, PA level and payload size
|
||||
are in `shared/HexapodProtocol` so they can't drift from the remote.
|
||||
the old sketch. Address, channel, data rate, PA level and the packet
|
||||
structs are in `shared/HexapodProtocol` so they can't drift from the
|
||||
remote.
|
||||
|
||||
## Bench-check checklist (do this before the first stand-up)
|
||||
|
||||
@@ -62,7 +63,11 @@ for your build:
|
||||
down).
|
||||
3. **RobotIMU register map** (`RobotIMU.cpp`) — written for an MPU6050-style
|
||||
chip (I2C addr `0x68`). If your gyro is a different part, its register
|
||||
addresses and scale factors will differ; swap them in.
|
||||
addresses and scale factors will differ; swap them in. Also check that
|
||||
it gives live readings at all (the robot's roll/pitch on the remote's
|
||||
STATUS page): the remote's MPU6050 answered and woke up but read all
|
||||
zeros until it got a full device reset (`PWR_MGMT_1 = 0x80`), which
|
||||
`RobotIMU::begin()` doesn't do.
|
||||
4. **Sit-pose assumption** — the built-in sit pose in `robot.ino` assumes all
|
||||
six feet stay grounded and simply lowers the body (a 0.4x height blend).
|
||||
If your chassis is meant to rest its belly on the ground instead, this
|
||||
@@ -83,7 +88,8 @@ for your build:
|
||||
|
||||
This exact code also compiles for the PC and runs in the Unity model. See
|
||||
`../sim/README.md`. `sim/build/sim_smoke.exe` is a quick headless check
|
||||
after every change.
|
||||
after every change. In Unity the simulated robot can also be driven by the
|
||||
real remote over the radio, through the USB nRF24 dongle.
|
||||
|
||||
## Testing without a remote yet
|
||||
|
||||
@@ -116,9 +122,27 @@ ignore walk commands.
|
||||
## Link with the remote
|
||||
|
||||
- `CmdPacket` and the `FLAG_*` bits in `shared/HexapodProtocol` are the
|
||||
exact wire format. Both radios set a static payload size of
|
||||
exact wire format. `Comms` drops any packet whose length isn't
|
||||
`sizeof(CmdPacket)`, so a robot and remote built from different protocol
|
||||
versions won't receive each other at all (and the robot fails safe).
|
||||
- **Telemetry back to the remote:** after every packet received, `Comms`
|
||||
loads a fresh `Telemetry` (robot state, IMU roll/pitch, battery, the 18
|
||||
commanded joint angles, packets per second, servo-cal echo) as the radio's
|
||||
ack payload. It goes back on the auto-ack of the remote's next packet:
|
||||
no extra transmissions. `sendTelemetry()` in `robot.ino` fills it every
|
||||
loop. The IMU attitude is now estimated whether or not robot gyro is on
|
||||
(the remote shows it); only the leveling is gated by `FLAG_ROBOT_GYRO`.
|
||||
- **Servo centre calibration** (remote's CAL page): while disarmed, a
|
||||
`cal_cmd` other than `CAL_NONE` puts the robot in SERVO_CAL. Every servo
|
||||
is driven to the centre pose (coxa 0, femur 0, tibia −90: legs straight
|
||||
out, shins straight down) and `CAL_SET` adjusts one servo's centre trim
|
||||
(µs, ±200, on top of `Config.h` `SERVOS`; it shifts the whole pulse range
|
||||
like the old `servoCallibration[]`). `CAL_SAVE` stores the 18 trims in
|
||||
EEPROM (`ServoDriver`, address 0, loaded at power-on), `CAL_REVERT`
|
||||
reloads them. `CAL_NONE`, arming or link loss relaxes the servos again.
|
||||
Put the robot on a stand first.
|
||||
- **Battery:** `Battery` reads `BATTERY_SENSE_PIN` through a divider
|
||||
(`Config.h`). Not wired yet (pin 255): the remote shows "--".
|
||||
- Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()`
|
||||
goes false and the main loop falls back to a zeroed `CmdPacket`, which
|
||||
disarms the robot.
|
||||
@@ -145,6 +169,7 @@ robot/
|
||||
├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend
|
||||
├── InputMapper.h/.cpp - CmdPacket -> twist/pose/mode decoding
|
||||
├── Limits.h/.cpp - workspace clamp, joint rate limit, stance polygon check
|
||||
├── Comms.h/.cpp - nRF24L01 receive (packet definition is in shared/HexapodProtocol)
|
||||
└── ServoDriver.h/.cpp - servo attach/relax/write
|
||||
├── Comms.h/.cpp - nRF24L01 receive + telemetry as ack payload (packets in shared/HexapodProtocol)
|
||||
├── ServoDriver.h/.cpp - servo PWM write/relax, per-servo centre trims (EEPROM), SERVO_CAL
|
||||
└── Battery.h/.cpp - battery voltage through a divider (not wired yet)
|
||||
```
|
||||
@@ -38,7 +38,7 @@ bool RobotIMU::readRaw(float& ax, float& ay, float& az, float& gx, float& gy, fl
|
||||
}
|
||||
|
||||
void RobotIMU::update(float dt) {
|
||||
if (!ok_ || !enabled) return;
|
||||
if (!ok_) return;
|
||||
float ax, ay, az, gx, gy, gz;
|
||||
if (!readRaw(ax, ay, az, gx, gy, gz)) return;
|
||||
|
||||
|
||||
@@ -4,11 +4,14 @@
|
||||
class RobotIMU {
|
||||
public:
|
||||
bool begin(); // returns false if the IMU didn't respond
|
||||
void update(float dt); // no-op if disabled or the IMU never came up
|
||||
void update(float dt); // no-op if the IMU never came up
|
||||
float rollDeg() const { return roll_deg_; }
|
||||
float pitchDeg() const { return pitch_deg_; }
|
||||
bool ok() const { return ok_; }
|
||||
|
||||
bool enabled = true; // robot-gyro toggle from CmdPacket sets this
|
||||
// Robot-gyro toggle from CmdPacket. The attitude is estimated either way
|
||||
// (the remote shows it); robot.ino only levels the body while it's set.
|
||||
bool enabled = true;
|
||||
|
||||
private:
|
||||
float roll_deg_ = 0, pitch_deg_ = 0;
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#include "ServoDriver.h"
|
||||
#include "Config.h"
|
||||
#include <HexapodProtocol.h>
|
||||
#include <EEPROM.h>
|
||||
|
||||
// 18 servos on hardware PWM, the way the old robot sketch drove them (the
|
||||
// Teensy 3.6 Servo library stops at 12). 16-bit duty at 50 Hz is ~0.3 us per
|
||||
@@ -28,10 +30,77 @@ void ServoDriver::relaxAll() {
|
||||
analogWrite(SERVOS[i][j].pin, 0); // no pulses: the servos go limp
|
||||
}
|
||||
|
||||
// ---- Centre trims ----
|
||||
// EEPROM record at address 0 (nothing else on the robot uses EEPROM). Bump
|
||||
// TRIM_VERSION if the layout changes - an old record is then ignored.
|
||||
static constexpr int TRIM_EEPROM_ADDR = 0;
|
||||
static constexpr uint32_t TRIM_MAGIC = 0x52545848; // "HXTR"
|
||||
static constexpr uint8_t TRIM_VERSION = 1;
|
||||
struct TrimRecord {
|
||||
uint32_t magic;
|
||||
uint8_t version;
|
||||
uint8_t checksum; // sum of the trim bytes
|
||||
int16_t trim_us[NUM_LEGS * 3];
|
||||
};
|
||||
|
||||
static int16_t g_trim[NUM_LEGS * 3] = {};
|
||||
static bool g_dirty = false;
|
||||
static bool g_stored = false;
|
||||
|
||||
static uint8_t trimChecksum(const int16_t* t) {
|
||||
uint8_t sum = 0;
|
||||
const uint8_t* b = reinterpret_cast<const uint8_t*>(t);
|
||||
for (size_t i = 0; i < sizeof(int16_t) * NUM_LEGS * 3; i++) sum += b[i];
|
||||
return sum;
|
||||
}
|
||||
|
||||
void ServoDriver::loadTrims() {
|
||||
TrimRecord r;
|
||||
EEPROM.get(TRIM_EEPROM_ADDR, r);
|
||||
g_stored = r.magic == TRIM_MAGIC && r.version == TRIM_VERSION && r.checksum == trimChecksum(r.trim_us);
|
||||
for (int i = 0; i < NUM_LEGS * 3; i++)
|
||||
g_trim[i] = g_stored ? constrain(r.trim_us[i], -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US) : 0;
|
||||
g_dirty = false;
|
||||
}
|
||||
|
||||
static void saveTrims() {
|
||||
TrimRecord r;
|
||||
r.magic = TRIM_MAGIC;
|
||||
r.version = TRIM_VERSION;
|
||||
memcpy(r.trim_us, g_trim, sizeof(g_trim));
|
||||
r.checksum = trimChecksum(r.trim_us);
|
||||
EEPROM.put(TRIM_EEPROM_ADDR, r);
|
||||
g_dirty = false;
|
||||
g_stored = true;
|
||||
}
|
||||
|
||||
int16_t ServoDriver::trim(int servo) { return (servo >= 0 && servo < NUM_LEGS * 3) ? g_trim[servo] : 0; }
|
||||
bool ServoDriver::trimsDirty() { return g_dirty; }
|
||||
bool ServoDriver::trimsStored() { return g_stored; }
|
||||
|
||||
const JointAngles ServoDriver::CAL_POSE = { 0.0f, 0.0f, -90.0f, true };
|
||||
|
||||
void ServoDriver::calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us) {
|
||||
bool valid_servo = servo < NUM_LEGS * 3;
|
||||
switch (cal_cmd) {
|
||||
case CAL_SET:
|
||||
if (valid_servo) {
|
||||
int16_t v = constrain(value_us, -CAL_TRIM_LIMIT_US, CAL_TRIM_LIMIT_US);
|
||||
if (v != g_trim[servo]) { g_trim[servo] = v; g_dirty = true; }
|
||||
}
|
||||
break;
|
||||
case CAL_SAVE: if (g_dirty) saveTrims(); break; // the remote repeats it: only write once
|
||||
case CAL_REVERT: if (g_dirty) loadTrims(); break;
|
||||
default: break; // CAL_HOLD
|
||||
}
|
||||
for (int i = 0; i < NUM_LEGS; i++) writeLeg(i, CAL_POSE);
|
||||
}
|
||||
|
||||
int ServoDriver::degToMicros(int leg, int joint, float deg) {
|
||||
const ServoCal& c = SERVOS[leg][joint];
|
||||
float us = c.us_at_0deg + c.us_per_deg * deg;
|
||||
return (int)constrain(us, (float)c.us_min, (float)c.us_max);
|
||||
int16_t t = g_trim[leg * 3 + joint];
|
||||
float us = c.us_at_0deg + t + c.us_per_deg * deg;
|
||||
return (int)constrain(us, (float)(c.us_min + t), (float)(c.us_max + t));
|
||||
}
|
||||
|
||||
void ServoDriver::writeLeg(int i, const JointAngles& a) {
|
||||
|
||||
@@ -7,6 +7,20 @@ public:
|
||||
void relaxAll(); // stop all pulses - call when disarming
|
||||
void writeLeg(int leg_index, const JointAngles& angles);
|
||||
|
||||
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS).
|
||||
// Joint angle -> pulse through that servo's calibration (Config.h SERVOS)
|
||||
// plus its centre trim.
|
||||
static int degToMicros(int leg, int joint, float deg);
|
||||
|
||||
// ---- Servo centre calibration (remote's CAL page, CmdPacket.cal_*) ----
|
||||
// Each servo has a centre trim in us on top of Config.h SERVOS, kept in
|
||||
// EEPROM. Trims shift the whole pulse range, like the old sketch's
|
||||
// servoCallibration[].
|
||||
static void loadTrims(); // from EEPROM; all 0 if none stored. Call in setup()
|
||||
static int16_t trim(int servo); // servo = leg * 3 + joint
|
||||
static bool trimsDirty(); // changed since the last load/save
|
||||
static bool trimsStored(); // the EEPROM holds a trim record
|
||||
// Applies one calibration command (CAL_* in HexapodProtocol.h) and holds
|
||||
// every servo at the centre pose. Call every loop while in SERVO_CAL.
|
||||
void calibrate(uint8_t cal_cmd, uint8_t servo, int16_t value_us);
|
||||
static const JointAngles CAL_POSE; // coxa 0, femur 0, tibia -90
|
||||
};
|
||||
@@ -9,6 +9,7 @@
|
||||
#include "Comms.h"
|
||||
#include "ServoDriver.h"
|
||||
#include "SmoothedValue.h"
|
||||
#include "Battery.h"
|
||||
|
||||
// Uncomment to drive the robot from the Serial Monitor before a remote exists.
|
||||
// See README.md for the key map.
|
||||
@@ -21,6 +22,7 @@ StateMachine state_machine;
|
||||
InputMapper mapper;
|
||||
Comms comms;
|
||||
ServoDriver servo_driver;
|
||||
Battery battery;
|
||||
|
||||
float current_heading_deg = 0; // open-loop: accumulates commanded omega. See note in loop().
|
||||
SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S); // remote height slider, rate-limited
|
||||
@@ -28,6 +30,7 @@ float gyro_corr_roll_deg = 0, gyro_corr_pitch_deg = 0; // robot-gyro self-level
|
||||
float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
|
||||
uint32_t last_loop_us = 0;
|
||||
RobotState last_state = RobotState::DISARMED;
|
||||
bool servo_cal_active = false; // SERVO_CAL: servos held at the centre pose for trimming
|
||||
|
||||
#ifdef TEST_SERIAL_INPUT
|
||||
CmdPacket test_packet;
|
||||
@@ -63,6 +66,33 @@ void pollSerialTestInput() {
|
||||
}
|
||||
#endif
|
||||
|
||||
// Robot state for the remote's display, sent back in the radio's ack payload.
|
||||
void sendTelemetry(const CmdPacket& cmd, const MapperOutput& mapped, RobotState state) {
|
||||
Telemetry t;
|
||||
t.roll_ddeg = (int16_t)lroundf(constrain(imu.rollDeg(), -3000.0f, 3000.0f) * 10);
|
||||
t.pitch_ddeg = (int16_t)lroundf(constrain(imu.pitchDeg(), -3000.0f, 3000.0f) * 10);
|
||||
t.battery_mv = battery.millivolts();
|
||||
t.state = servo_cal_active ? TSTATE_SERVO_CAL : (uint8_t)state;
|
||||
t.status = (imu.ok() ? TSTAT_IMU_OK : 0) |
|
||||
(mapped.robot_gyro_enabled ? TSTAT_GYRO_ON : 0) |
|
||||
(state != RobotState::DISARMED || servo_cal_active ? TSTAT_SERVOS_ON : 0) |
|
||||
(ServoDriver::trimsDirty() ? TSTAT_CAL_DIRTY : 0) |
|
||||
(ServoDriver::trimsStored() ? TSTAT_TRIMS_STORED : 0);
|
||||
t.cal_servo = cmd.cal_servo;
|
||||
t.cal_trim_us = ServoDriver::trim(cmd.cal_servo);
|
||||
t.rx_per_sec = comms.packetsPerSec();
|
||||
for (int i = 0; i < NUM_LEGS; i++) {
|
||||
const float* j = last_joint_deg[i];
|
||||
float c = servo_cal_active ? ServoDriver::CAL_POSE.coxa_deg : j[0];
|
||||
float f = servo_cal_active ? ServoDriver::CAL_POSE.femur_deg : j[1];
|
||||
float b = servo_cal_active ? ServoDriver::CAL_POSE.tibia_deg : j[2];
|
||||
t.joint_deg[i * 3] = (int8_t)constrain(lroundf(c), -127L, 127L);
|
||||
t.joint_deg[i * 3 + 1] = (int8_t)constrain(lroundf(f), -127L, 127L);
|
||||
t.joint_deg[i * 3 + 2] = (int8_t)constrain(lroundf(b) + TELEMETRY_TIBIA_OFFSET_DEG, -127L, 127L);
|
||||
}
|
||||
comms.setTelemetry(t);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
gait.begin();
|
||||
@@ -73,6 +103,7 @@ void setup() {
|
||||
if (!imu.begin()) Serial.println("robot IMU not responding - stabilization disabled");
|
||||
if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only");
|
||||
|
||||
ServoDriver::loadTrims();
|
||||
servo_driver.begin();
|
||||
last_loop_us = micros();
|
||||
}
|
||||
@@ -115,6 +146,18 @@ void loop() {
|
||||
} else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
|
||||
servo_driver.begin();
|
||||
}
|
||||
// Servo centre calibration (remote CAL page): only while disarmed. The
|
||||
// servos hold the centre pose through their trims; leaving it - CAL_NONE,
|
||||
// arming or link loss (cmd is then a default packet) - relaxes them again.
|
||||
bool servo_cal = !mapped.armed && cmd.cal_cmd != CAL_NONE;
|
||||
if (servo_cal) {
|
||||
if (!servo_cal_active) servo_driver.begin();
|
||||
servo_driver.calibrate(cmd.cal_cmd, cmd.cal_servo, cmd.cal_value);
|
||||
} else if (servo_cal_active && state == RobotState::DISARMED) {
|
||||
servo_driver.relaxAll();
|
||||
}
|
||||
servo_cal_active = servo_cal;
|
||||
|
||||
// The stand pose holds every foot at neutral, so walking starts from there.
|
||||
if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet();
|
||||
last_state = state;
|
||||
@@ -203,4 +246,7 @@ void loop() {
|
||||
servo_driver.writeLeg(i, angles);
|
||||
}
|
||||
}
|
||||
|
||||
battery.update(dt);
|
||||
sendTelemetry(cmd, mapped, state);
|
||||
}
|
||||
@@ -0,0 +1,283 @@
|
||||
"""Builds remote/remote.HMI, the remote's Nextion touchscreen project, from code.
|
||||
|
||||
The layout below is the single source of truth for the HMI: change it here and
|
||||
re-run, rather than editing remote.HMI in Nextion Editor (an Editor edit is
|
||||
lost on the next run). Component names and event code must match the
|
||||
firmware - remote/NextionUI.cpp and remote/NEXTION.md.
|
||||
|
||||
python scripts\\make_remote_hmi.py (from 2026-current, PowerShell 5.1 fine)
|
||||
|
||||
Needs:
|
||||
- the nxhmi package: the tools/nextion-hmi-tools submodule (git submodule update --init)
|
||||
- Pillow, and the DejaVu Sans Mono fonts (free licence). They ship with
|
||||
matplotlib; or pass --font-dir with DejaVuSansMono.ttf + DejaVuSansMono-Bold.ttf.
|
||||
- the old remote HMI (../2019-teensy-nrf24/remote-nextion/remote.HMI), read
|
||||
only: its splash picture is reused.
|
||||
Then open remote.HMI in Nextion Editor, compile and upload (SETUP > UPDATE
|
||||
DISPLAY puts the remote in USB passthrough for that).
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
import tempfile
|
||||
|
||||
HERE = os.path.dirname(os.path.abspath(__file__))
|
||||
ROOT = os.path.dirname(HERE) # 2026-current
|
||||
sys.path.insert(0, os.path.join(ROOT, "tools", "nextion-hmi-tools", "src"))
|
||||
from nxhmi import mcp_server as nx # noqa: E402 (the same functions the nxhmi MCP server exposes)
|
||||
|
||||
OUT = os.path.join(ROOT, "remote", "remote.HMI")
|
||||
OLD_HMI = os.path.join(ROOT, "..", "2019-teensy-nrf24", "remote-nextion", "remote.HMI")
|
||||
|
||||
W, H = 480, 320
|
||||
BAR_H = 32 # top bar
|
||||
TAB_Y, TAB_H = 272, 48
|
||||
BIG, SMALL = 0, 1 # font ids: 24 px bold, 16 px
|
||||
|
||||
# Colours. The firmware repaints the stateful ones (remote/NextionUI.cpp):
|
||||
# gray #c6c3c6 = off, green #00fc00 = on, red #f80000, orange #f88000.
|
||||
PAGE_BG = "#182030"
|
||||
BAR_BG = "#000000"
|
||||
TEXT = "#ffffff"
|
||||
LABEL = "#a0b0c8"
|
||||
BTN = "#c6c3c6"
|
||||
RED = "#f80000"
|
||||
TRACK = "#404850"
|
||||
TAB_ON = "#3060c0"
|
||||
|
||||
# Page order = page id; must match UiPage in remote/UiTypes.h.
|
||||
PAGES = ["splash", "drive", "status", "servos", "cal", "setup", "upload"]
|
||||
TABS = [("drive", "DRIVE"), ("status", "STATUS"), ("servos", "SERVOS"), ("cal", "CAL"), ("setup", "SETUP")]
|
||||
|
||||
LEG_NAMES = ["FR", "MR", "RR", "RL", "ML", "FL"] # robot leg order (robot/Config.h)
|
||||
LEG_TITLES = ["FRONT RIGHT", "MID RIGHT", "REAR RIGHT", "REAR LEFT", "MID LEFT", "FRONT LEFT"]
|
||||
|
||||
|
||||
def frame(cmd, value=None):
|
||||
"""printh line for a custom '#' frame (remote/NextionUI.h)."""
|
||||
body = [ord(cmd)] + ([value] if value is not None else [])
|
||||
return "printh 23 %02X %s" % (len(body), " ".join("%02X" % b for b in body))
|
||||
|
||||
|
||||
class Builder:
|
||||
def __init__(self, path):
|
||||
self.path = path
|
||||
|
||||
def add(self, page, kind, name, events=None, **att):
|
||||
nx.t_add_component(self.path, page, kind, name, att, events or {})
|
||||
|
||||
def text(self, page, name, x, y, w, h, txt, font=SMALL, pco=TEXT, bco=PAGE_BG, xcen="left", ycen="center",
|
||||
maxl=None, **kw):
|
||||
self.add(page, "text", name, x=x, y=y, w=w, h=h, txt=txt, font=font, pco=pco, bco=bco,
|
||||
xcen=xcen, ycen=ycen, txt_maxl=maxl or max(len(txt), 10), **kw)
|
||||
|
||||
def button(self, page, name, x, y, w, h, txt, up=None, font=BIG, bco=BTN, pco="#000000", maxl=None, **kw):
|
||||
events = {"codesup": up} if up else {}
|
||||
self.add(page, "button", name, events, x=x, y=y, w=w, h=h, txt=txt, font=font, bco=bco, bco2=bco,
|
||||
pco=pco, pco2=pco, txt_maxl=maxl or max(len(txt), 10), **kw)
|
||||
|
||||
def slider(self, page, name, x, y, w, h, maxval, val, events=None, cursor=None):
|
||||
self.add(page, "slider", name, events, x=x, y=y, w=w, h=h, bco=TRACK, bco1=TRACK, pco=TEXT,
|
||||
wid=cursor or h, hig=h, maxval=maxval, minval=0, val=val)
|
||||
|
||||
def bar(self, page, name, x, y, w, h, val=0):
|
||||
self.add(page, "progress_bar", name, x=x, y=y, w=w, h=h, bco=TRACK, pco="#00c000", val=val)
|
||||
|
||||
# ---- shared frame: top bar + tabs on every working page ----
|
||||
def chrome(self, page):
|
||||
self.text(page, "tLink", 0, 0, 112, BAR_H, "NO LINK", font=SMALL, pco=RED, bco=BAR_BG, xcen="center", maxl=12)
|
||||
self.text(page, "tState", 112, 0, 256, BAR_H, "ROBOT: NO DATA", font=SMALL, pco=TEXT, bco=BAR_BG, maxl=30)
|
||||
self.button(page, "bKill", 368, 0, 112, BAR_H, "DISARM", [frame("K")], font=BIG, bco=RED, pco=TEXT)
|
||||
tw = W // len(TABS)
|
||||
for i, (target, label) in enumerate(TABS):
|
||||
on = target == page
|
||||
self.button(page, "bTab%d" % i, i * tw, TAB_Y, tw, TAB_H, label, ["page %s" % target], font=SMALL,
|
||||
bco=TAB_ON if on else "#283040", pco=TEXT, borderw=1)
|
||||
|
||||
def page(self, name, preinit=()):
|
||||
"""Every page reports itself to the remote when it opens ('P' + page id)."""
|
||||
if name != PAGES[0]:
|
||||
nx.t_add_page(self.path, name, {"bco": PAGE_BG, "sta": "solid color"})
|
||||
else:
|
||||
nx.t_set_attributes(self.path, name, name, {"bco": PAGE_BG})
|
||||
nx.t_set_event(self.path, name, name, "codesload", [frame("P", PAGES.index(name))] + list(preinit))
|
||||
|
||||
|
||||
def splash_png(tmp):
|
||||
"""The old remote's splash picture (a jumping spider), rendered from the old HMI."""
|
||||
from nxhmi.render import Renderer
|
||||
from nxhmi.container import HMIFile
|
||||
img = Renderer(HMIFile.load(OLD_HMI)).render("splashsceen")
|
||||
path = os.path.join(tmp, "splash.png")
|
||||
img.save(path)
|
||||
return path
|
||||
|
||||
|
||||
def font_dir(arg):
|
||||
if arg:
|
||||
return arg
|
||||
import matplotlib
|
||||
return os.path.join(os.path.dirname(matplotlib.__file__), "mpl-data", "fonts", "ttf")
|
||||
|
||||
|
||||
MODEL = "NX4832T035_011"
|
||||
|
||||
|
||||
def new_project_flipped(path, program):
|
||||
"""Like nxhmi's project.new_project, but landscape turned 180 degrees (Editor: Setting >
|
||||
Display direction 270): the display sits upside down in the remote (owner, 2026-11-17).
|
||||
Must come first: pictures and fonts added afterwards are stored for this direction."""
|
||||
from nxhmi import models, project
|
||||
from nxhmi.components import new_page
|
||||
direction = models.direction_byte(MODEL, landscape=True, flipped=True)
|
||||
w, h = models.page_size(MODEL, direction)
|
||||
items = [("main.HMI", project.make_main(MODEL, [("pa", "0.pa")], "iso-8859-1", direction)),
|
||||
("Program.s", program.encode("latin-1")),
|
||||
("0.pa", new_page(PAGES[0], w=w, h=h).pack())]
|
||||
project.build(items).save(path)
|
||||
|
||||
|
||||
def build(path, fonts):
|
||||
b = Builder(path)
|
||||
program = ("//Runs once at power-on, before the first page.\r\n"
|
||||
"bauds=115200 //the remote's NEXTION_BAUD; stored in the display\r\n"
|
||||
"page 0\r\n")
|
||||
new_project_flipped(path, program)
|
||||
nx.t_add_font(path, 24, os.path.join(fonts, "DejaVuSansMono-Bold.ttf"), "iso-8859-1") # font 0
|
||||
nx.t_add_font(path, 16, os.path.join(fonts, "DejaVuSansMono.ttf"), "iso-8859-1") # font 1
|
||||
with tempfile.TemporaryDirectory() as tmp:
|
||||
pic = nx.t_add_picture(path, splash_png(tmp))["picture_id"]
|
||||
|
||||
# ---------------------------------------------------------------- 0 splash
|
||||
p = "splash"
|
||||
b.page(p)
|
||||
nx.t_set_attributes(path, p, p, {"sta": "image", "pic": pic})
|
||||
b.text(p, "t0", 0, 0, 480, 40, "HEXAPOD REMOTE", font=BIG, pco=TEXT, bco="#000000", xcen="center", sta="crop image", picc=pic)
|
||||
b.add(p, "timer", "tm0", {"codestimer": ["page drive"]}, tim=1500, en="enable")
|
||||
|
||||
for name in PAGES[1:]:
|
||||
b.page(name, preinit=PREINIT.get(name, ()))
|
||||
for name, _ in TABS:
|
||||
b.chrome(name)
|
||||
|
||||
# ---------------------------------------------------------------- 1 drive
|
||||
p = "drive"
|
||||
y = 40
|
||||
for i, (n, t, c) in enumerate([("bArm", "DISARMED", "A"), ("bStand", "SIT", "S"),
|
||||
("bGait", "TRIPOD", "G"), ("bMode", "DIRECT", "M")]):
|
||||
b.button(p, n, 4 + i * 119, y, 115, 48, t, [frame(c)], maxl=10)
|
||||
y = 94
|
||||
for i, (n, t, c) in enumerate([("bRs", "R: ROTATE", "X"), ("bRGyro", "REMOTE GYRO", "R"), ("bBGyro", "ROBOT GYRO", "B")]):
|
||||
b.button(p, n, 4 + i * 159, y, 154, 44, t, [frame(c)], font=SMALL, maxl=12)
|
||||
b.text(p, "tSpL", 4, 146, 80, 36, "SPEED", font=SMALL, pco=LABEL)
|
||||
send_speed = ["printh 23 02 56", "prints hSpeed.val,1"] # 'V' + value byte
|
||||
b.slider(p, "hSpeed", 86, 150, 300, 28, 255, 128, {"codesslide": send_speed, "codesup": send_speed}, cursor=20)
|
||||
b.text(p, "tSpeed", 394, 146, 82, 36, "50%", font=BIG, xcen="right", maxl=6)
|
||||
b.text(p, "tHtL", 4, 190, 80, 36, "HEIGHT", font=SMALL, pco=LABEL)
|
||||
send_height = ["printh 23 02 48", "prints hHeight.val,1"] # 'H' + value byte
|
||||
b.slider(p, "hHeight", 86, 194, 300, 28, 200, 100, {"codesslide": send_height, "codesup": send_height}, cursor=20)
|
||||
b.text(p, "tHeight", 394, 190, 82, 36, "+0%", font=BIG, xcen="right", maxl=6)
|
||||
b.text(p, "tMsg", 4, 234, 472, 34, "", font=SMALL, pco="#ffd040", maxl=40)
|
||||
|
||||
# ---------------------------------------------------------------- 2 status
|
||||
p = "status"
|
||||
for side, x, prefix in (("REMOTE (deg)", 4, "R"), ("ROBOT (deg)", 248, "B")):
|
||||
b.text(p, "tHdr" + prefix, x, 36, 228, 22, side, font=SMALL, pco=LABEL)
|
||||
b.text(p, "t%sRoll" % prefix, x, 58, 228, 22, "roll --", maxl=16)
|
||||
b.slider(p, "h%sRoll" % prefix, x, 80, 228, 16, 90, 45, cursor=10)
|
||||
b.text(p, "t%sPitch" % prefix, x, 100, 228, 22, "pitch --", maxl=16)
|
||||
b.slider(p, "h%sPitch" % prefix, x, 122, 228, 16, 90, 45, cursor=10)
|
||||
b.text(p, "tHdrS", 4, 144, 60, 22, "STICKS", pco=LABEL)
|
||||
for i, axis in enumerate(("X", "Y", "TWIST")): # centred over the slider columns below
|
||||
x = 32 + i * 150
|
||||
if i == 0:
|
||||
x, w = 67, 70 # same centre (102), clear of "STICKS"
|
||||
else:
|
||||
w = 140
|
||||
b.text(p, "tAx%d" % i, x, 144, w, 22, axis, pco=LABEL, xcen="center")
|
||||
for row, (label, y) in enumerate((("L", 168), ("R", 192))):
|
||||
b.text(p, "tSt" + label, 4, y, 24, 20, label, pco=LABEL)
|
||||
for i, axis in enumerate("XYT"):
|
||||
b.slider(p, "h%s%s" % (label, axis), 32 + i * 150, y + 2, 140, 16, 200, 100, cursor=10)
|
||||
b.text(p, "tBatt", 4, 220, 472, 24, "battery remote -- robot --", maxl=40)
|
||||
b.text(p, "tRadio", 4, 244, 472, 24, "radio no link", maxl=44)
|
||||
|
||||
# ---------------------------------------------------------------- 3 servos
|
||||
p = "servos"
|
||||
# Top view of the robot, front up: right legs on the right.
|
||||
slots = {5: (4, 36), 4: (4, 114), 3: (4, 192), 0: (304, 36), 1: (304, 114), 2: (304, 192)}
|
||||
for leg, (x, y) in slots.items():
|
||||
b.text(p, "tLeg%d" % leg, x, y, 172, 18, LEG_TITLES[leg], pco=LABEL, maxl=12)
|
||||
for j in range(3):
|
||||
i = leg * 3 + j
|
||||
b.text(p, "tJ%d" % i, x, y + 19 + j * 19, 108, 18, ["coxa", "femur", "tibia"][j] + " --", maxl=12)
|
||||
b.bar(p, "jJ%d" % i, x + 110, y + 22 + j * 19, 62, 13)
|
||||
b.text(p, "tBody", 196, 40, 88, 200, "FRONT", font=SMALL, pco=TEXT, bco="#303848", xcen="center",
|
||||
ycen="up", style="border", borderc="#8090a8", borderw=2)
|
||||
b.text(p, "tSrv", 196, 246, 88, 22, "NO DATA", font=SMALL, pco="#808080", xcen="center", maxl=10)
|
||||
|
||||
# ---------------------------------------------------------------- 4 cal
|
||||
p = "cal"
|
||||
b.button(p, "bCalOn", 4, 38, 130, 44, "START", [frame("O")], maxl=6)
|
||||
b.text(p, "tMsg", 140, 38, 336, 44, "Robot on a stand, disarmed: START", font=SMALL, pco="#ffd040", maxl=40, isbr="true")
|
||||
for i in range(6):
|
||||
b.button(p, "bL%d" % i, 4 + i * 79, 88, 75, 40, LEG_NAMES[i], [frame("L", i)], maxl=4)
|
||||
for j, n in enumerate(["COXA", "FEMUR", "TIBIA"]):
|
||||
b.button(p, "bJ%d" % j, 4 + j * 158, 134, 154, 40, n, [frame("J", j)], maxl=6)
|
||||
for n, x, t, code in (("bM10", 4, "-10", 0), ("bM1", 78, "-1", 1), ("bP1", 332, "+1", 2), ("bP10", 406, "+10", 3)):
|
||||
b.button(p, n, x, 180, 70, 44, t, [frame("T", code)], maxl=4)
|
||||
b.text(p, "tTrim", 152, 180, 176, 44, "FR coxa", font=SMALL, xcen="center", maxl=20, bco="#303848")
|
||||
b.button(p, "bZeroT", 4, 228, 110, 40, "ZERO", [frame("T", 4)], font=SMALL, maxl=6)
|
||||
b.button(p, "bRevert", 118, 228, 140, 40, "REVERT", [frame("E")], font=SMALL, maxl=8)
|
||||
b.button(p, "bSave", 262, 228, 214, 40, "SAVE TO ROBOT", [frame("W")], font=SMALL, maxl=14)
|
||||
|
||||
# ---------------------------------------------------------------- 5 setup
|
||||
p = "setup"
|
||||
b.text(p, "tHdr1", 4, 36, 472, 22, "STICKS AND HANDSET", pco=LABEL, maxl=20)
|
||||
b.button(p, "bCal", 4, 60, 170, 44, "CAL STICKS", [frame("C")], font=SMALL, maxl=12)
|
||||
b.button(p, "bCalX", 180, 60, 120, 44, "CANCEL", [frame("Q")], font=SMALL, maxl=8)
|
||||
b.button(p, "bZero", 306, 60, 170, 44, "LEVEL REMOTE", [frame("Z")], font=SMALL, maxl=14)
|
||||
b.text(p, "tHdr2", 4, 110, 472, 22, "DISPLAY", pco=LABEL, maxl=20)
|
||||
b.button(p, "bUpd", 4, 134, 230, 44, "UPDATE DISPLAY", [frame("U")], font=SMALL, maxl=16)
|
||||
b.text(p, "tDim", 244, 134, 70, 44, "LIGHT", pco=LABEL)
|
||||
b.slider(p, "hDim", 314, 142, 162, 28, 100, 100, {"codesslide": ["dim=hDim.val"], "codesup": ["dims=hDim.val"]},
|
||||
cursor=20)
|
||||
b.text(p, "tVer", 4, 186, 472, 22, "remote fw --", pco=LABEL, maxl=50)
|
||||
b.text(p, "tMsg", 4, 212, 472, 54, "", pco="#ffd040", maxl=40, isbr="true")
|
||||
|
||||
# ---------------------------------------------------------------- 6 upload
|
||||
p = "upload"
|
||||
b.text(p, "t0", 20, 40, 440, 50, "DISPLAY UPDATE MODE", font=BIG, xcen="center", maxl=20)
|
||||
for i, line in enumerate(("Upload the .tft from Nextion Editor", "over the remote's USB port.", "",
|
||||
"Power-cycle the remote when done.")):
|
||||
b.text(p, "t%d" % (i + 1), 20, 110 + i * 26, 440, 24, line, xcen="center", maxl=40)
|
||||
return path
|
||||
|
||||
|
||||
# Page pre-initialize code after the 'P' frame.
|
||||
PREINIT = {
|
||||
# Indicator sliders: no touch, so a finger can't drag them.
|
||||
"status": ["tsw hRRoll,0", "tsw hRPitch,0", "tsw hBRoll,0", "tsw hBPitch,0",
|
||||
"tsw hLX,0", "tsw hLY,0", "tsw hLT,0", "tsw hRX,0", "tsw hRY,0", "tsw hRT,0"],
|
||||
"setup": ["hDim.val=dim"],
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser(description=__doc__.split("\n")[0])
|
||||
ap.add_argument("--out", default=OUT)
|
||||
ap.add_argument("--font-dir", help="folder with DejaVuSansMono.ttf and DejaVuSansMono-Bold.ttf")
|
||||
a = ap.parse_args()
|
||||
tmp = a.out + ".new"
|
||||
if os.path.exists(tmp):
|
||||
os.remove(tmp)
|
||||
build(tmp, font_dir(a.font_dir))
|
||||
problems = nx.t_verify(tmp)
|
||||
if not problems["ok"]:
|
||||
sys.exit("verify failed: %s" % problems["problems"])
|
||||
os.replace(tmp, a.out)
|
||||
print("wrote", a.out)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -2,14 +2,15 @@
|
||||
#include <Arduino.h>
|
||||
|
||||
// =====================================================================
|
||||
// Remote -> robot wire protocol. SINGLE SOURCE OF TRUTH.
|
||||
// Remote <-> robot wire protocol. SINGLE SOURCE OF TRUTH.
|
||||
//
|
||||
// Both robot/ and remote/ compile against this exact header (PlatformIO
|
||||
// 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
|
||||
// rebuild and reflash BOTH boards together. Each end drops any packet whose
|
||||
// length isn't sizeof() of what it expects, so a remote and robot running
|
||||
// mismatched versions simply won't hear each other (the robot then fails
|
||||
// safe on link timeout) instead of misinterpreting fields.
|
||||
// Telemetry (below) comes back from the robot in the radio's ack payloads.
|
||||
//
|
||||
// Axis convention (right-handed body frame, viewed from above, like ROS
|
||||
// REP-103): +X forward, +Y left, +Z up. Positive rotations follow the
|
||||
@@ -21,6 +22,10 @@
|
||||
// remote never needs to know robot geometry.
|
||||
// =====================================================================
|
||||
|
||||
// Protocol version: bump on any change to CmdPacket, Telemetry or the radio
|
||||
// settings below (the payload sizes are checked on both ends as well).
|
||||
constexpr uint8_t HXP_PROTOCOL_VERSION = 4;
|
||||
|
||||
struct CmdPacket {
|
||||
int16_t vx = 0; // left stick: + forward / - backward
|
||||
int16_t vy = 0; // left stick: + strafe left / - strafe right
|
||||
@@ -29,15 +34,18 @@ struct CmdPacket {
|
||||
int16_t remote_roll = 0; // remote handset tilt (feedforward), only honored with FLAG_REMOTE_GYRO
|
||||
int16_t remote_pitch = 0;
|
||||
int16_t body_height = 0; // standing height setting: -1000 lowest, 0 = default, +1000 highest
|
||||
int16_t cal_value = 0; // servo calibration: centre trim in us for cal_servo (CAL_SET only)
|
||||
uint8_t speed_scale = 128; // 0-255, multiplies vx/vy
|
||||
uint8_t flags = 0; // bit-packed, see FLAG_* below
|
||||
uint8_t seq = 0; // increments every transmitted packet
|
||||
uint8_t cal_cmd = 0; // servo calibration command, CAL_* below. 0 = normal operation
|
||||
uint8_t cal_servo = 0xFF; // servo calibration: selected servo, leg * 3 + joint (0..17)
|
||||
};
|
||||
|
||||
// Both ends are Teensy (Cortex-M7, little-endian, 2-byte int16 alignment):
|
||||
// 9 x int16 + 3 x uint8 = 21 bytes, padded to 22. If a different MCU family
|
||||
// Both ends are Teensy (Cortex-M, little-endian, 2-byte int16 alignment):
|
||||
// 10 x int16 + 5 x uint8 = 25 bytes, padded to 26. If a different MCU family
|
||||
// ever joins, pack the struct explicitly rather than trusting this.
|
||||
static_assert(sizeof(CmdPacket) == 22, "CmdPacket layout changed - reflash both boards");
|
||||
static_assert(sizeof(CmdPacket) == 26, "CmdPacket layout changed - reflash both boards");
|
||||
static_assert(sizeof(CmdPacket) <= 32, "nRF24L01 payloads are limited to 32 bytes");
|
||||
|
||||
constexpr uint8_t FLAG_ARMED = 1 << 0;
|
||||
@@ -53,10 +61,63 @@ constexpr uint8_t FLAG_GAIT_WAVE = 1 << 5; // 0 = tripod requested, 1 = wav
|
||||
constexpr uint8_t FLAG_REMOTE_GYRO = 1 << 6; // remote tilt feedforward enabled
|
||||
constexpr uint8_t FLAG_ROBOT_GYRO = 1 << 7; // robot stabilization feedback enabled
|
||||
|
||||
// ---- Servo centre calibration (CmdPacket.cal_cmd) ----
|
||||
// Only honoured while FLAG_ARMED is clear. Any CAL_* other than CAL_NONE puts
|
||||
// the robot in its SERVO_CAL state: every servo is driven to its centre pose
|
||||
// (coxa 0, femur 0, tibia -90 = legs straight out, shins straight down) through
|
||||
// the per-servo trims, so a mis-centred horn shows. Every command is a state,
|
||||
// not an event, so a repeated or lost packet does no harm. Leaving SERVO_CAL
|
||||
// (CAL_NONE, arming, or link loss) relaxes the servos again.
|
||||
constexpr uint8_t CAL_NONE = 0; // normal operation
|
||||
constexpr uint8_t CAL_HOLD = 1; // hold the centre pose; select cal_servo, report its trim
|
||||
constexpr uint8_t CAL_SET = 2; // ... and set cal_servo's trim to cal_value (us)
|
||||
constexpr uint8_t CAL_SAVE = 3; // ... and store all trims in the robot's EEPROM (if changed)
|
||||
constexpr uint8_t CAL_REVERT = 4; // ... and reload all trims from the EEPROM (drop unsaved changes)
|
||||
constexpr int16_t CAL_TRIM_LIMIT_US = 200; // trims are clamped to +-this
|
||||
constexpr uint8_t CAL_NO_SERVO = 0xFF;
|
||||
|
||||
// =====================================================================
|
||||
// Robot -> remote telemetry. The robot preloads this as the nRF24 ACK
|
||||
// PAYLOAD, so it rides back on the auto-ack of the remote's next packet:
|
||||
// no extra transmissions, and nothing changes for the remote -> robot
|
||||
// direction. Ack payloads need dynamic payload lengths on both radios, so
|
||||
// each end checks the received length against sizeof() itself and drops
|
||||
// anything else - a remote and robot on different protocol versions still
|
||||
// don't understand each other's packets.
|
||||
// =====================================================================
|
||||
struct Telemetry {
|
||||
int16_t roll_ddeg = 0; // robot IMU roll, 0.1 deg (+ = right side down)
|
||||
int16_t pitch_ddeg = 0; // robot IMU pitch, 0.1 deg (+ = nose down)
|
||||
uint16_t battery_mv = 0; // robot battery, mV. 0 = not measured
|
||||
int16_t cal_trim_us = 0; // centre trim of cal_servo, us
|
||||
uint8_t state = 0; // TSTATE_* below
|
||||
uint8_t status = 0; // TSTAT_* bits below
|
||||
uint8_t cal_servo = CAL_NO_SERVO; // echo of CmdPacket.cal_servo (the trim above belongs to it)
|
||||
uint8_t rx_per_sec = 0; // remote packets the robot received in the last second
|
||||
int8_t joint_deg[18] = {}; // commanded joint angles, leg * 3 + joint, degrees.
|
||||
// Tibia is stored + 90 (its range -150..-20 doesn't fit an int8):
|
||||
// tibia = joint_deg[i * 3 + 2] - 90.
|
||||
};
|
||||
static_assert(sizeof(Telemetry) == 30, "Telemetry layout changed - reflash both boards");
|
||||
static_assert(sizeof(Telemetry) <= 32, "nRF24L01 ack payloads are limited to 32 bytes");
|
||||
constexpr int TELEMETRY_TIBIA_OFFSET_DEG = 90;
|
||||
|
||||
constexpr uint8_t TSTATE_DISARMED = 0; // same numbering as the robot's RobotState
|
||||
constexpr uint8_t TSTATE_SIT = 1;
|
||||
constexpr uint8_t TSTATE_STAND = 2;
|
||||
constexpr uint8_t TSTATE_WALKING = 3;
|
||||
constexpr uint8_t TSTATE_SERVO_CAL = 4;
|
||||
|
||||
constexpr uint8_t TSTAT_IMU_OK = 1 << 0; // robot IMU responded at power-on
|
||||
constexpr uint8_t TSTAT_GYRO_ON = 1 << 1; // robot-gyro self-leveling active
|
||||
constexpr uint8_t TSTAT_SERVOS_ON = 1 << 2; // servos are being driven (not relaxed)
|
||||
constexpr uint8_t TSTAT_CAL_DIRTY = 1 << 3; // servo trims changed since the last save
|
||||
constexpr uint8_t TSTAT_TRIMS_STORED = 1 << 4; // the EEPROM holds a trim record
|
||||
|
||||
// ---- Radio link settings (must be identical on both ends) ----
|
||||
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);
|
||||
constexpr uint8_t HXP_RADIO_PAYLOAD = sizeof(CmdPacket); // remote -> robot (checked per packet, see Telemetry)
|
||||
// Data rate / PA level are RF24 enums; kept as plain ints here so this header
|
||||
// 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.
|
||||
|
||||
@@ -83,6 +83,7 @@ HXSIM_API void hxsim_step(uint32_t dt_us) {
|
||||
}
|
||||
|
||||
HXSIM_API void hxsim_radio_send(const uint8_t* bytes, int32_t len) { simhw::radioSend(bytes, len); }
|
||||
HXSIM_API int32_t hxsim_get_ack_payload(uint8_t* out, int32_t cap) { return simhw::radioLastAckPayload(out, cap); }
|
||||
|
||||
HXSIM_API void hxsim_set_imu(float ax, float ay, float az, float gx, float gy, float gz) {
|
||||
g_manual_imu = true;
|
||||
|
||||
@@ -18,7 +18,7 @@
|
||||
#endif
|
||||
|
||||
// Bump whenever a signature or struct below changes; the C# side checks it.
|
||||
#define HXSIM_API_VERSION 7
|
||||
#define HXSIM_API_VERSION 8
|
||||
|
||||
#define HXSIM_NUM_LEGS 6
|
||||
|
||||
@@ -83,6 +83,10 @@ HXSIM_API void hxsim_get_servos(int32_t* us_18, int32_t* attached_18, uint32_
|
||||
HXSIM_API void hxsim_get_joints_deg(float* deg_18); // servo pulses decoded through Config.h's per-servo SERVOS table
|
||||
HXSIM_API void hxsim_get_feet_mm(float* xyz_18); // forward kinematics of those angles, body frame
|
||||
HXSIM_API void hxsim_get_status(HxStatus* out);
|
||||
// The robot's telemetry (shared/HexapodProtocol Telemetry) as it came back in
|
||||
// the ack payload of the last packet received. Returns its length in bytes
|
||||
// (sizeof(Telemetry)), 0 before the first one.
|
||||
HXSIM_API int32_t hxsim_get_ack_payload(uint8_t* out, int32_t cap);
|
||||
// Kinematic body (SimBody.cpp): the body rests on the support plane of its
|
||||
// feet and grounded feet don't slide. World frame = robot frame at power-on,
|
||||
// ground at z = 0. Any pointer may be null.
|
||||
|
||||
+31
-18
@@ -19,7 +19,7 @@ robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 3.6 firmw
|
||||
|---|---|
|
||||
| `analogWrite()` hardware PWM (how `ServoDriver` drives the servos) | the pulse width per pin, worked out from duty, resolution and frequency, then decoded back to angles through `Config.h`'s per-servo `SERVOS` table |
|
||||
| `Wire` + MPU6050 registers | a register-level fake MPU6050 at 0x68, fed from the simulated body's tilt and rotation |
|
||||
| `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match, and the RX FIFO holds 3 |
|
||||
| `RF24` | a simulated radio: packets are received only if channel, address, data rate and **payload size** match (any length up to 32 once `enableAckPayload()` turns on dynamic payloads, as on the chip), and the RX FIFO holds 3. `writeAckPayload()` goes back with the next packet received: `hxsim_get_ack_payload` returns it (the robot's `Telemetry`) |
|
||||
| `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O |
|
||||
|
||||
`robot_ino.cpp` compiles `robot.ino` itself and exposes read-only views of
|
||||
@@ -65,31 +65,43 @@ It drives the firmware the way the remote would and checks:
|
||||
- **`+vx` moves the body forward**,
|
||||
- link loss disarms,
|
||||
- a wrong-size packet is ignored,
|
||||
- the serial harness still works.
|
||||
- the robot's telemetry comes back in the ack payload, and every servo
|
||||
calibration command (HOLD / SET / SAVE / REVERT, arming refused) works,
|
||||
- the serial harness still works,
|
||||
- plus the body-pose, leg-clearance, heading and robot-gyro checks listed
|
||||
in the file.
|
||||
|
||||
## Using it in Unity
|
||||
|
||||
Open `Hexapod_Simulation_Unity` and press Play in `Main.unity`. No scene
|
||||
edits are needed: `HexapodSimBootstrap` adds a `HexapodSimulation` object,
|
||||
disables the old movement scripts (`Walk`, `RotationControl`, `Leg`,
|
||||
`HandleHoystick`), and binds the existing leg rig by name (`Leg` → `hip1`
|
||||
/ `Hip2` / `Knie`). The on-screen panel (F1 toggles it) offers three input
|
||||
sources:
|
||||
disables the old movement scripts (`Walk`, `RotationControl`, `Leg`), and
|
||||
binds the existing leg rig by name (`Leg` → `hip1` / `Hip2` / `Knie`). By
|
||||
default `OpenScadModel` swaps the old scene's robot for the OpenSCAD model
|
||||
of the real one (`HexapodSimulation.useOpenScadModel`). The on-screen panel
|
||||
(F1 toggles it) offers two input sources:
|
||||
|
||||
- **Joystick:** the real remote as a USB controller, on input axes
|
||||
`"1"`–`"6"` like the old `HandleHoystick`. Axis assignment and inversion
|
||||
are set on the `SimRemote` component. The panel shows the raw axes so you
|
||||
can fix them.
|
||||
- **Keyboard:** test without a remote. WASD walks, Q/E turns, IJKL + U/O
|
||||
drive the right stick, 1–7 flip the toggles, Backspace disarms, +/-
|
||||
change speed.
|
||||
- **Robot serial:** no radio at all. Keystrokes go to `robot.ino`'s own
|
||||
`TEST_SERIAL_INPUT` harness.
|
||||
change speed. The panel has every remote toggle, the speed slider and
|
||||
simulated handset tilt (remote gyro).
|
||||
- **Real remote (radio):** the real handheld remote, over the air. The USB
|
||||
nRF24 dongle (`../dongle/`, bridge mode) receives its packets, they go
|
||||
into the firmware byte for byte, and the robot's `Telemetry` goes back as
|
||||
the radio's ack payload, exactly like the real robot's `Comms`. The
|
||||
remote's display then shows the simulated robot (state, IMU, joint
|
||||
angles, servo calibration). `DongleLink.cs` finds the dongle's CH340 port
|
||||
by itself (or set `SimRemote.donglePort`, e.g. `COM7`); quit SignalRGB
|
||||
first, it grabs CH340 ports. Don't run the real robot at the same time:
|
||||
both would answer the remote.
|
||||
|
||||
The panel also has every remote toggle, the speed slider, simulated
|
||||
handset tilt (remote gyro), a **ground tilt** slider (to test the robot
|
||||
gyro), and live firmware state, joint angles, foot contact, the packet on
|
||||
air and robot serial output.
|
||||
There's no USB link to the remote or the robot any more (the old joystick
|
||||
and serial-harness inputs and the `Serial*` / `HandleHoystick` scripts are
|
||||
gone): everything goes over the radio.
|
||||
|
||||
The panel also has a **ground tilt** slider (to test the robot gyro), and
|
||||
live firmware state, joint angles, foot contact, the packet on air and the
|
||||
robot's debug prints.
|
||||
|
||||
The body moves **kinematically**, computed in the DLL (`SimBody.cpp`), so
|
||||
Unity shows what the smoke test checks:
|
||||
@@ -119,4 +131,5 @@ component to the scene yourself.
|
||||
firmware *intends*, including pulse-limit clamping (which is how it
|
||||
caught the old ±90° coxa offsets). Whether a real servo turns that way is
|
||||
a bench check.
|
||||
- The remote firmware. `SimRemote.cs` mirrors its packet rules instead.
|
||||
- The remote firmware, in Keyboard mode: `SimRemote.cs` mirrors its packet
|
||||
rules instead. With the real remote over the radio, its own firmware runs.
|
||||
@@ -213,6 +213,9 @@ struct AirPacket {
|
||||
static bool g_radio_present = true;
|
||||
static std::deque<AirPacket> g_air; // packets waiting at the receiver
|
||||
static constexpr size_t RX_FIFO_DEPTH = 3;
|
||||
static std::vector<uint8_t> g_ack_queued; // robot's writeAckPayload(), not sent yet
|
||||
static bool g_ack_has_queued = false;
|
||||
static std::vector<uint8_t> g_ack_delivered; // came back with the last packet received
|
||||
|
||||
namespace simhw {
|
||||
void radioSetPresent(bool present) { g_radio_present = present; }
|
||||
@@ -225,6 +228,16 @@ void radioSend(const uint8_t* bytes, int len) {
|
||||
memcpy(p.address, HXP_RADIO_ADDRESS, 5);
|
||||
p.payload.assign(bytes, bytes + len);
|
||||
g_air.push_back(p);
|
||||
// The chip acks on reception, carrying whatever ack payload is queued.
|
||||
if (g_ack_has_queued) {
|
||||
g_ack_delivered = g_ack_queued;
|
||||
g_ack_has_queued = false;
|
||||
}
|
||||
}
|
||||
int radioLastAckPayload(uint8_t* out, int cap) {
|
||||
int n = (int)g_ack_delivered.size() < cap ? (int)g_ack_delivered.size() : cap;
|
||||
if (n > 0) memcpy(out, g_ack_delivered.data(), n);
|
||||
return n;
|
||||
}
|
||||
} // namespace simhw
|
||||
|
||||
@@ -240,7 +253,7 @@ bool RF24::available() {
|
||||
while (!g_air.empty()) {
|
||||
const AirPacket& p = g_air.front();
|
||||
bool match = p.channel == channel_ && p.rate == data_rate_ &&
|
||||
memcmp(p.address, address_, 5) == 0 && p.payload.size() == payload_size_;
|
||||
memcmp(p.address, address_, 5) == 0 && (dynamic_ || p.payload.size() == payload_size_);
|
||||
if (match) return true;
|
||||
g_air.pop_front();
|
||||
}
|
||||
@@ -252,3 +265,15 @@ void RF24::read(void* buf, uint8_t len) {
|
||||
memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size());
|
||||
g_air.pop_front();
|
||||
}
|
||||
uint8_t RF24::getDynamicPayloadSize() { return g_air.empty() ? 0 : (uint8_t)g_air.front().payload.size(); }
|
||||
bool RF24::writeAckPayload(uint8_t, const void* buf, uint8_t len) {
|
||||
if (!dynamic_ || len > 32) return false;
|
||||
const uint8_t* b = static_cast<const uint8_t*>(buf);
|
||||
g_ack_queued.assign(b, b + len);
|
||||
g_ack_has_queued = true;
|
||||
return true;
|
||||
}
|
||||
uint8_t RF24::flush_tx() {
|
||||
g_ack_has_queued = false;
|
||||
return 0;
|
||||
}
|
||||
@@ -32,5 +32,8 @@ void imuSet(float ax, float ay, float az, float gx, float gy, float gz);
|
||||
void radioSetPresent(bool present);
|
||||
// Puts one packet "on air" using the protocol's channel/address/data rate.
|
||||
void radioSend(const uint8_t* bytes, int len);
|
||||
// The ack payload that came back with the last packet received (what the
|
||||
// remote would read after write()). Returns its length, 0 if none yet.
|
||||
int radioLastAckPayload(uint8_t* out, int cap);
|
||||
|
||||
} // namespace simhw
|
||||
@@ -37,6 +37,7 @@ inline double degrees(double rad) { return rad * (180.0 / PI); }
|
||||
void pinMode(uint8_t pin, uint8_t mode);
|
||||
void digitalWrite(uint8_t pin, uint8_t value);
|
||||
void analogWrite(uint8_t pin, int value);
|
||||
inline int analogRead(uint8_t) { return 0; } // nothing analog is simulated
|
||||
uint32_t analogWriteResolution(uint32_t bits); // returns the previous resolution, like Teensyduino
|
||||
void analogWriteFrequency(uint8_t pin, float hz);
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
// Not used by robot/ today; here so a future robot-side EEPROM use doesn't
|
||||
// break the simulation build silently. Backed by RAM, starts erased (0xFF).
|
||||
// ServoDriver keeps its servo centre trims here. Backed by RAM, starts erased (0xFF),
|
||||
// so every simulated power-on starts with no trims stored.
|
||||
#include <Arduino.h>
|
||||
|
||||
class SimEEPROM {
|
||||
|
||||
@@ -8,8 +8,11 @@ enum rf24_datarate_e { RF24_1MBPS = 0, RF24_2MBPS, RF24_250KBPS };
|
||||
|
||||
// A simulated radio. Packets the simulation puts "on air" (hxsim_radio_send)
|
||||
// are only received if this radio's channel, data rate, address and static
|
||||
// payload size match the sender's, just like the real nRF24L01. The RX FIFO
|
||||
// holds 3 packets; extra packets are dropped.
|
||||
// payload size match the sender's, just like the real nRF24L01 (with dynamic
|
||||
// payloads - enableAckPayload() - any length up to 32 is received). The RX
|
||||
// FIFO holds 3 packets; extra packets are dropped. An ack payload queued with
|
||||
// writeAckPayload() goes back with the next packet received, like the chip's
|
||||
// auto-ack (hxsim_get_ack_payload shows it).
|
||||
class RF24 {
|
||||
public:
|
||||
RF24(uint16_t ce_pin, uint16_t csn_pin) { (void)ce_pin; (void)csn_pin; }
|
||||
@@ -27,6 +30,11 @@ public:
|
||||
bool available();
|
||||
void read(void* buf, uint8_t len);
|
||||
bool write(const void*, uint8_t) { return false; } // the robot never transmits
|
||||
void enableDynamicPayloads() { dynamic_ = true; }
|
||||
void enableAckPayload() { dynamic_ = true; }
|
||||
uint8_t getDynamicPayloadSize();
|
||||
bool writeAckPayload(uint8_t pipe, const void* buf, uint8_t len);
|
||||
uint8_t flush_tx();
|
||||
|
||||
private:
|
||||
uint8_t payload_size_ = 32;
|
||||
@@ -35,4 +43,5 @@ private:
|
||||
uint8_t address_[5] = {};
|
||||
bool pipe_open_ = false;
|
||||
bool listening_ = false;
|
||||
bool dynamic_ = false;
|
||||
};
|
||||
@@ -227,6 +227,21 @@ int main() {
|
||||
snprintf(buf, sizeof(buf), "%u servo writes in 1.5 s (0 = every IK solution was rejected as invalid)", writes);
|
||||
check(writes > 0, "IK produces valid joint solutions at the stance pose", buf);
|
||||
printLegs("standing:");
|
||||
{
|
||||
Telemetry t;
|
||||
int len = hxsim_get_ack_payload(reinterpret_cast<uint8_t*>(&t), sizeof(t));
|
||||
float deg[18]; hxsim_get_joints_deg(deg);
|
||||
float worst = 0;
|
||||
for (int i = 0; i < 18; i++) {
|
||||
float tj = t.joint_deg[i] - (i % 3 == 2 ? TELEMETRY_TIBIA_OFFSET_DEG : 0);
|
||||
worst = fmaxf(worst, fabsf(tj - deg[i]));
|
||||
}
|
||||
snprintf(buf, sizeof(buf), "%d bytes, state %u, status %02X, joints off by up to %.1f deg, %u packets/s",
|
||||
len, t.state, t.status, worst, t.rx_per_sec);
|
||||
check(len == (int)sizeof(Telemetry) && t.state == TSTATE_STAND && (t.status & TSTAT_SERVOS_ON) &&
|
||||
worst < 1.5f && t.rx_per_sec >= 90,
|
||||
"telemetry comes back in the ack payload (state, servo angles, packet rate)", buf);
|
||||
}
|
||||
|
||||
float feet[18]; hxsim_get_feet_mm(feet);
|
||||
float worst = 0;
|
||||
@@ -553,6 +568,68 @@ int main() {
|
||||
s = status();
|
||||
check(!s.link_ok && s.robot_state == 0, "link loss (>0.3 s silence) disarms the robot");
|
||||
|
||||
// ---- Servo centre calibration (CAL_* commands, only while disarmed) ----
|
||||
{
|
||||
auto tele = []() { Telemetry t; hxsim_get_ack_payload(reinterpret_cast<uint8_t*>(&t), sizeof(t)); return t; };
|
||||
int us[18], att[18]; uint32_t wr[18];
|
||||
CmdPacket c; // disarmed
|
||||
c.cal_cmd = CAL_HOLD;
|
||||
c.cal_servo = 4; // leg 1 (mid-right) femur
|
||||
run(c, 0.3f);
|
||||
float deg[18]; hxsim_get_joints_deg(deg);
|
||||
hxsim_get_servos(us, att, wr);
|
||||
float worst = 0; int relaxed = 0;
|
||||
for (int i = 0; i < 18; i++) {
|
||||
worst = fmaxf(worst, fabsf(deg[i] - (i % 3 == 2 ? -90.0f : 0.0f)));
|
||||
if (!att[i]) relaxed++;
|
||||
}
|
||||
Telemetry t = tele();
|
||||
snprintf(buf, sizeof(buf), "state %u, %d relaxed, pose off by up to %.1f deg", t.state, relaxed, worst);
|
||||
check(t.state == TSTATE_SERVO_CAL && relaxed == 0 && worst < 1.0f,
|
||||
"CAL_HOLD holds every servo at the centre pose (coxa 0, femur 0, tibia -90)", buf);
|
||||
int us0 = us[4];
|
||||
|
||||
c.cal_cmd = CAL_SET; c.cal_value = 50;
|
||||
run(c, 0.2f);
|
||||
hxsim_get_servos(us, att, wr);
|
||||
t = tele();
|
||||
snprintf(buf, sizeof(buf), "pulse %d -> %d us, echo servo %u trim %d, status %02X", us0, us[4], t.cal_servo, t.cal_trim_us, t.status);
|
||||
check(abs(us[4] - us0 - 50) <= 1 && t.cal_servo == 4 && t.cal_trim_us == 50 && (t.status & TSTAT_CAL_DIRTY),
|
||||
"CAL_SET trims the selected servo and the telemetry echoes it", buf);
|
||||
|
||||
c.cal_cmd = CAL_SAVE;
|
||||
run(c, 0.2f);
|
||||
t = tele();
|
||||
check((t.status & TSTAT_TRIMS_STORED) && !(t.status & TSTAT_CAL_DIRTY), "CAL_SAVE stores the trims");
|
||||
|
||||
c.cal_cmd = CAL_SET; c.cal_value = -120;
|
||||
run(c, 0.2f);
|
||||
c.cal_cmd = CAL_REVERT;
|
||||
run(c, 0.2f);
|
||||
t = tele();
|
||||
snprintf(buf, sizeof(buf), "trim %d after revert", t.cal_trim_us);
|
||||
check(t.cal_trim_us == 50 && !(t.status & TSTAT_CAL_DIRTY), "CAL_REVERT drops unsaved trims", buf);
|
||||
|
||||
c.flags = FLAG_ARMED; // arming wins over calibration
|
||||
run(c, 0.3f);
|
||||
t = tele();
|
||||
check(t.state == TSTATE_SIT, "arming while CAL_* is sent leaves SERVO_CAL (robot arms into SIT)");
|
||||
|
||||
c.flags = 0; c.cal_cmd = CAL_SET; c.cal_value = 0; // put the trim back for the checks below
|
||||
run(c, 0.2f);
|
||||
c.cal_cmd = CAL_SAVE;
|
||||
run(c, 0.2f);
|
||||
c.cal_cmd = CAL_NONE;
|
||||
run(c, 0.2f);
|
||||
hxsim_get_servos(us, att, wr);
|
||||
relaxed = 0;
|
||||
for (int i = 0; i < 18; i++) if (!att[i]) relaxed++;
|
||||
t = tele();
|
||||
snprintf(buf, sizeof(buf), "state %u, %d of 18 relaxed, trim %d", t.state, relaxed, t.cal_trim_us);
|
||||
check(t.state == TSTATE_DISARMED && relaxed == 18 && t.cal_trim_us == 0, "CAL_NONE relaxes the servos again", buf);
|
||||
runSilent(0.5f);
|
||||
}
|
||||
|
||||
// ---- Protocol mismatch fails closed ----
|
||||
uint8_t short_payload[16] = {};
|
||||
short_payload[10] = FLAG_ARMED;
|
||||
|
||||
Loaded 100 of 102 files, more files were not shown because too many files have changed in this diff.
Show more
Reference in new issue
Block a user