Firmware simulation, Unity project, and sim-verified robot fixes
Simulation - sim/: the unchanged robot firmware compiled into hexapod_sim.dll against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050, RF24 with matching-config radio), a kinematic body model (support plane + planted-foot odometry) that also drives the simulated IMU, and a headless smoke test (sim_smoke). build.ps1 builds and tests. - Hexapod_Simulation_Unity/ folded in (history kept in a bundle outside the repo). Assets/HexapodSim loads the DLL, drives the existing leg rig, and offers joystick / keyboard / robot-serial input plus a test panel. Robot (all found or confirmed in the simulation) - Config.h geometry taken from the Unity model. - IK femur elevation sign, gait stance direction, body pose rotation direction, yaw lever arm. - Stance feet integrated from the current command; gait restarts at mid-stance; heading integrates the applied turn rate, only while walking. - Leg clearance and reach checks (Limits::feetFeasible); walk command and body pose scaled as a whole, pose scale rate-limited, planted feet guarded so the body never snaps or legs collide. - Robot-gyro self-leveling as an integrator. - Heading mode: steering relative to the body; S walks backward. - Standing height from the remote (STANCE_HEIGHT_RANGE_MM). Protocol v3 (reflash both boards) - CmdPacket gains body_height (22 bytes). Remote - Height slider (Nextion hHeight/nHeight, serial [ ]). - Fixed unspecified I2C read order in RemoteIMU. LEGACY/: owner's old Nextion HMI and remote hardware assets. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
91dac16fca
commit
935526eab5
145 files changed
+431258
-114
No files matched your search
+166
-20
@@ -2,11 +2,22 @@
|
||||
|
||||
Hexapod walking robot plus a custom handheld remote, both Teensy-based and
|
||||
linked one-way over nRF24L01 (remote → robot, with hardware auto-ack only).
|
||||
This file is the persistent context for working on either side. Read it
|
||||
fully before changing anything.
|
||||
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.
|
||||
|
||||
## Layout and toolchain
|
||||
|
||||
```
|
||||
robot/ Teensy 4.0 firmware (PlatformIO env robot)
|
||||
remote/ Teensy 4.1 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/)
|
||||
LEGACY/ owner's older material for reference (remote/remote.HMI = old Nextion UI, 6 pages)
|
||||
scripts/ PlatformIO helper
|
||||
```
|
||||
|
||||
PlatformIO, one `platformio.ini`, two environments:
|
||||
|
||||
| Env | Board | Sketch dir | Purpose |
|
||||
@@ -29,7 +40,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
|
||||
## Communication contract (`shared/HexapodProtocol/HexapodProtocol.h`)
|
||||
|
||||
`CmdPacket`, 20 bytes (static_asserted), sent remote → robot at 100 Hz
|
||||
`CmdPacket`, 22 bytes (static_asserted; protocol v3), sent remote → robot at 100 Hz
|
||||
(robot `CONTROL_HZ` is 60):
|
||||
|
||||
| Field | Range | Meaning |
|
||||
@@ -39,6 +50,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
| `yaw_rate` | ±1000 | + turn left (CCW). Ignored by the robot in mode B |
|
||||
| `rx, ry, rz` | ±1000 | right stick: roll/pitch/yaw, or X/Y/Z offset when `FLAG_RSTICK_XLATE` |
|
||||
| `remote_roll, remote_pitch` | ±1000 | handset tilt feedforward, used only when `FLAG_REMOTE_GYRO` |
|
||||
| `body_height` | ±1000 | standing height setting: `STANCE_HEIGHT_MM` ± `STANCE_HEIGHT_RANGE_MM` (150 ± 50), 0 = default; the robot rate-limits it (`STANCE_HEIGHT_RATE_MM_S`) and everything built on `Kinematics::neutralStance()` follows |
|
||||
| `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) |
|
||||
@@ -76,12 +88,12 @@ joint-rate clamp → servos.
|
||||
| `Config.h` | **all placeholder geometry/pins/rates** |
|
||||
| `SmoothedValue.h` | rate-limited setpoint smoother used for every discrete command |
|
||||
| `Kinematics` | per-leg 3-DOF analytic IK, `neutralStance()` |
|
||||
| `GaitEngine` | tripod/wave patterns, per-leg phase, pattern swap only at touchdown |
|
||||
| `BodyPose` | 6-axis body pose (smoothed command + IMU roll/pitch correction) applied to foot targets |
|
||||
| `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` |
|
||||
| `StateMachine` | DISARMED → SIT → STAND → WALKING, sit/stand blend at 1/s. Disarm is instant |
|
||||
| `InputMapper` | CmdPacket → Twist + PoseCommand + mode flags. Mode B = heading-hold from stick angle. Adds remote tilt on top of stick pose when `FLAG_REMOTE_GYRO`, clamped to `MAX_TILT_DEG` |
|
||||
| `Limits` | workspace clamp (70 mm horiz / 60 mm vert), joint rate clamp, ≥3-legs-in-stance check (not acted on yet) |
|
||||
| `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` | attach/detach/write, skips invalid IK solutions |
|
||||
|
||||
@@ -110,15 +122,74 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
= CCW. The mapping into the packet is in `PacketBuilder.cpp` (e.g.
|
||||
`vy = -L_X`, translate mode `ry = -R_X`).
|
||||
|
||||
## Simulation (`sim/` + `Hexapod_Simulation_Unity/`), details in `sim/README.md`
|
||||
|
||||
- **Principle:** the robot firmware is compiled unmodified for the PC. No
|
||||
`#ifdef SIM` in `robot/`, ever. Hardware is replaced beneath it by
|
||||
`sim/shim/` stand-ins (`Arduino.h`, `Servo.h`, `Wire.h` with a
|
||||
register-level MPU6050, `RF24.h` with a matching-config radio,
|
||||
`SPI.h`, `EEPROM.h`).
|
||||
- **Build:** `.\sim\build.ps1` (configure + build + smoke test; uses
|
||||
g++/cmake/ninja from `C:\Strawberry\c\bin` if they're not on PATH).
|
||||
Rebuild after any `robot/` or `shared/` change.
|
||||
- **The dev PC is Windows, and the owner uses Windows PowerShell 5.1.**
|
||||
Every documented command must work there: no `&&`, one command per line,
|
||||
or a `.ps1` script.
|
||||
- **Gotchas (already solved; don't undo them):**
|
||||
- `robot/` must never be on any include path, not even `-iquote`. On
|
||||
Windows' case-insensitive filesystem GCC's `<limits.h>` chain would pick
|
||||
up `robot/Limits.h`. So each `robot/*.cpp` is compiled through a
|
||||
generated wrapper in `sim/build/robot_src/`, and sim sources include
|
||||
`"../robot/..."` directly.
|
||||
- The shim's `min`/`max` return `std::common_type` by value; a `decltype`
|
||||
return type gave dangling references.
|
||||
- `robot_ino.cpp` compiles `robot.ino` and exposes read-only views of its
|
||||
globals (`SimRobotAccess.h`). The API is in `HexapodSimApi.h`
|
||||
(`HXSIM_API_VERSION`, mirrored by `HexapodSimNative.ExpectedApiVersion`
|
||||
in C#). The FK in `HexapodSimApi.cpp` is deliberately independent of
|
||||
`robot/Kinematics.cpp`.
|
||||
- **Joint convention the sim (and Config.h limits) assume:** coxa + = CCW
|
||||
from the mount angle; femur = elevation, + = up; tibia relative to the
|
||||
femur, + = up (knee-down stance is about −98°). Rig mapping:
|
||||
`hip1.y = −(coxa + mount offset)`, `Hip2.x = −femur`,
|
||||
`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.
|
||||
- `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.
|
||||
- `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.
|
||||
The ground is the support plane (a plane all feet are on or above;
|
||||
prefer most contacts, then continuity with the last ground), and 2D
|
||||
Kabsch on the feet that stayed grounded gives planar motion. The
|
||||
simulated MPU6050 is fed from it after every control tick. Ground tilt
|
||||
comes from `hxsim_set_ground_tilt`. The old C# "lowest feet + plane
|
||||
fit" model flipped the body as soon as it rolled (only one side
|
||||
counted as grounded); don't reintroduce it.
|
||||
- Robot forward = Unity −X, 1 Unity unit = 10 mm.
|
||||
- The Unity project was folded in from its own repo. Its history is in
|
||||
`D:\Claude\Hexapod_Simulation_Unity-history.bundle`. Its nested `.git`
|
||||
must be removed by the owner (deleting it was blocked for Claude).
|
||||
|
||||
## Testing without the other board
|
||||
|
||||
- **Robot without remote:** `#define TEST_SERIAL_INPUT` in `robot.ino` (on
|
||||
by default). Serial 115200, single keys: `a` arm, `s` sit/stand, `w/x`
|
||||
vx ±200, `q/e` vy ±200 (left/right), `z/c` yaw_rate ∓/±200, `g` gait,
|
||||
vx ±200, `q/e` vy ±200 (left/right), `z/c` yaw_rate ±200 (left/right), `g` gait,
|
||||
`m` mode, space = zero velocity. Keys inject through
|
||||
`Comms::injectPacket()`, the same path radio packets take. Order: arm →
|
||||
stand → walk. The harness does not exercise the right stick or remote
|
||||
tilt. Add keys if needed.
|
||||
- **Robot firmware in simulation:** `sim/build/sim_smoke.exe` (headless,
|
||||
scripted checks), or Unity Play mode with Keyboard / Joystick / Robot
|
||||
serial input.
|
||||
- **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
|
||||
@@ -128,20 +199,86 @@ Remote stick-frame convention: logical X + = right, Y + = forward, twist +
|
||||
## Placeholders and bench-check items (not yet verified on hardware)
|
||||
|
||||
Robot (`robot/README.md` has detail):
|
||||
- All of `Config.h`: leg mount angles/positions, link lengths, joint
|
||||
limits, stance radius/height, servo pins, µs/deg calibration,
|
||||
`MAX_JOINT_DEG_S` (in robot.ino), Limits envelope, InputMapper maxima.
|
||||
- `Config.h` geometry now comes from the Unity model (owner's request),
|
||||
1 unit = 10 mm:
|
||||
- legs at X ±100 / 0, Y ±50 / ±65 mm,
|
||||
- mount angles ±90° (coxa straight sideways at servo center),
|
||||
- coxa/femur/tibia 55/100/150 mm,
|
||||
- stance radius 75 (so 130 mm out from the coxa pivot), height 150 mm.
|
||||
|
||||
The old scene's `Leg.translateX` = ±5 was only the old code's rotation
|
||||
pivot; the rig hips are at x ±10. Still to confirm on the real chassis:
|
||||
joint limits, servo pins, µs/deg calibration, `MAX_JOINT_DEG_S` (in
|
||||
robot.ino), the Limits envelope, InputMapper maxima.
|
||||
- **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
|
||||
−98.3°, feet exactly at −150 mm. `sim_smoke` guards this.
|
||||
- **Servo calibration model is too simple for this robot:** a single
|
||||
`SERVO_US_AT_0DEG` for all joints means tibia 0° (straight) = servo
|
||||
center, but the old Unity/servo code shows this build's knee servo
|
||||
center is tibia −90°. Left-side servos are also mirrored in the old code.
|
||||
Per-joint offset/direction would be needed in `ServoDriver` (robot code
|
||||
change, not made).
|
||||
- **Pin conflict:** `SERVO_PINS` leg 3 uses pins 11 and 12, which are
|
||||
Teensy 4.0 SPI MOSI/MISO needed by the nRF24. Must be moved. Some servo
|
||||
pins (26, 27, 30…) may also not be PWM-capable on the 4.0. Verify.
|
||||
- IK knee direction (`tibia_rad` sign), BodyPose roll/pitch sign, IMU
|
||||
register map, and the sit pose assuming feet stay grounded.
|
||||
- **Suspected gait direction inversion:** in `GaitEngine::update` the
|
||||
stance phase moves the foot from `neutral - half` to `neutral + half`,
|
||||
i.e. with the commanded direction. That pushes the body *backward* for
|
||||
+vx, and the same happens for yaw. Fix = swap start/end. Flagged to the
|
||||
owner, **not changed**. The harness `z`/`c` yaw keys currently rely on
|
||||
this behavior and need rechecking after the fix.
|
||||
- **Fixed after the owner saw them in Unity (sim_smoke now guards each):**
|
||||
- Right-stick rotations were inverted: `BodyPose::apply` rotated the feet
|
||||
by the pose instead of by its inverse. This also made the robot-gyro
|
||||
correction positive feedback.
|
||||
- Heading mode was off. The body turned at only 41% of the commanded ω
|
||||
(the gait used the hip as the yaw lever arm instead of the foot). The
|
||||
heading estimate also integrated while standing. Stance feet were
|
||||
interpolated between endpoints recomputed each tick, so a changing
|
||||
command undid part of the motion. And walking started with the feet at
|
||||
neutral but at arbitrary phases, so grounded legs overran the workspace
|
||||
clamp.
|
||||
- Now: mode A turns at about 95% of command, and mode B turns 83° of a
|
||||
commanded 90°. The rest is open-loop estimate error; fusing IMU yaw is
|
||||
the next step.
|
||||
- Combined right-stick poses (e.g. full roll+pitch+yaw) used to clamp
|
||||
each foot on its own, distorting the planted feet by up to 29 mm. Now
|
||||
the whole pose is reach-scaled (`BodyPose::fitToReach`). sim_smoke
|
||||
checks that four full-deflection combos keep the feet rigid within
|
||||
3 mm with no frozen servos. The Unity panel shows the scale
|
||||
("body pose 63% (leg reach limit)"), via `HxStatus.pose_reach_scale`
|
||||
(API version 2).
|
||||
- Legs could collide (full-rate turning needs ~60 mm half-strides, and
|
||||
neighbouring feet are ~100 mm apart), and walking while posing made
|
||||
the pose scale saw with the step cycle. Both are now handled by leg
|
||||
clearance in `Limits::feetFeasible` plus `GaitEngine::limitTwist` and
|
||||
envelope-based `fitToReach`. sim_smoke checks clearance with its own
|
||||
FK, and checks rigidity and steadiness while walking and posing. The
|
||||
panel shows "walk NN%" (`HxStatus.twist_scale`, API version 3).
|
||||
- Walking + rotate keys still freaked out. Changing the command
|
||||
mid-step ran planted feet out of reach, and the pose scale *snapped*
|
||||
down, which the per-joint rate clamp then tore out of formation. Now:
|
||||
- `GaitEngine::update` moves all planted feet by one common factor
|
||||
that keeps them feasible (with the pose applied), and the heading
|
||||
integrates `appliedTwist()`.
|
||||
- The pose scale moves at a bounded rate both ways
|
||||
(`REACH_DROP_PER_SEC` 3/s, recover 1/s).
|
||||
- While walking, the pose fits the requested stride and the current
|
||||
feet (walking has priority), and the walk is limited again against
|
||||
the current pose (`limitTwist(..., pose, chained)`), so it slows
|
||||
during the handover.
|
||||
- sim_smoke's "toggling rotate keys" check mimics a keyboard.
|
||||
- Robot-gyro self-leveling was `correction = -measured tilt`, which only
|
||||
halves a slope because the measurement includes the correction. It's
|
||||
now an integrator (`ROBOT_GYRO_KI` 3/s, clamped to
|
||||
`ROBOT_GYRO_MAX_CORR_DEG`) on (measured tilt − operator's commanded
|
||||
tilt). sim_smoke: an 8° ground is levelled to < 1° without
|
||||
oscillation.
|
||||
- **Fixed (owner-approved):** the `GaitEngine::update` stance direction.
|
||||
Stance now runs from `neutral + half` to `neutral - half`, so the feet
|
||||
push opposite to the commanded motion. The harness `z`/`c` keys were
|
||||
corrected to `z` = `yaw_rate` + (turn left). `sim_smoke` checks that
|
||||
+vx goes forward and +yaw_rate turns left.
|
||||
- `RobotIMU.cpp` reads `(Wire.read() << 8) | Wire.read()`, and C++ leaves
|
||||
the order of those two calls unspecified (fixed in the remote's copy).
|
||||
- Heading for mode B is open-loop (integrated commanded ω). Fusing IMU yaw
|
||||
is a planned upgrade.
|
||||
- `Limits::stancePolygonOk()` is computed but not acted on.
|
||||
@@ -153,15 +290,24 @@ Remote (`remote/README.md` has detail):
|
||||
- `IMU_AXIS_FROM_CHIP` / `IMU_AXIS_SIGN` depend on how the MPU6050 is
|
||||
mounted in the handset.
|
||||
- The Nextion must have `bauds=115200` in Program.s, and the HMI must match
|
||||
`NEXTION.md`. No `.HMI` file is in the repo yet. The compiled `.tft` is
|
||||
gitignored.
|
||||
`NEXTION.md`. The owner's old 6-page HMI is in `LEGACY/remote/remote.HMI`;
|
||||
its component names aren't readable outside Nextion Editor. Adapting
|
||||
`NextionUI` to it needs the owner's component list, or the old remote
|
||||
Teensy code, which the owner will add to `LEGACY/` when found. The
|
||||
compiled `.tft` is gitignored.
|
||||
- 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
|
||||
|
||||
- The **robot side is owner-verified code**. Don't edit `robot/` unless
|
||||
something is factually wrong relative to the protocol, and ask the owner
|
||||
first even then. Flag suspected robot bugs instead of fixing them.
|
||||
first even then. Flag suspected robot bugs instead of fixing them. To
|
||||
try a robot fix, copy `robot/ shared/ sim/` to a scratch dir, patch the
|
||||
copy and run `sim_smoke` there.
|
||||
- `LEGACY/` is reference material from the owner. Don't modify it.
|
||||
- Protocol changes go in `shared/HexapodProtocol/` only. Update the size
|
||||
`static_assert`, this file's contract table, and both READMEs.
|
||||
- Match the existing style: config constants at the top of `Config.h`
|
||||
|
||||
Reference in new issue
Block a user