Robot: Teensy 3.6 with the old robot's servo wiring and calibration
- LEGACY/Hexapod_Teensy/: old robot sketch (reference only). - platformio.ini: robot env targets teensy36 (owner-confirmed board). - Config.h: per-servo SERVOS table (pin, pulse at 0 deg, signed us/deg, pulse limits) built from the old sketch's pins, ranges, trims and the old Unity servo directions. Rear-left/front-left coxa offsets set to 0: the owner re-horns those two servos (the old +-90 left them at the end of travel; the sim showed them clamping). - ServoDriver: 18 servos on hardware PWM (50 Hz, 16-bit); the Teensy 3.6 Servo library stops at 12. - Radio on CE 39 / CSN 15 with SPI SCK on pin 14. - sim: analogWrite/PWM shims, pulses decoded through SERVOS (API v6). - Docs: CLAUDE.md, READMEs. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
mangavalkandClaude Opus 5.5 committed
1 parent
a961fa721b
commit
5ff85c4eb8
21 files changed
+795
-80
No files matched your search
+56
-14
@@ -9,13 +9,14 @@ before changing anything.
|
||||
## Layout and toolchain
|
||||
|
||||
```
|
||||
robot/ Teensy 4.0 firmware (PlatformIO env robot)
|
||||
robot/ Teensy 3.6 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;
|
||||
remote/teensy/ = old remote sketch from https://mangavalk.com/Hexapod/remote.git @ 492dece)
|
||||
remote/teensy/ = old remote sketch from https://mangavalk.com/Hexapod/remote.git @ 492dece;
|
||||
Hexapod_Teensy/ = old robot sketch: servo passthrough only, no IK - see "Old robot sketch" below)
|
||||
scripts/ PlatformIO helper
|
||||
```
|
||||
|
||||
@@ -23,7 +24,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
|
||||
| Env | Board | Sketch dir | Purpose |
|
||||
|-----|-------|------------|---------|
|
||||
| `robot` | `teensy40` | `robot/` | walking robot |
|
||||
| `robot` | `teensy36` | `robot/` | walking robot |
|
||||
| `remote` | `teensy41` | `remote/` | handheld transmitter |
|
||||
|
||||
- `shared/` is PlatformIO's `lib_dir`. `shared/HexapodProtocol/` is a
|
||||
@@ -77,7 +78,7 @@ PlatformIO, one `platformio.ini`, two environments:
|
||||
pose happen on the robot. Don't duplicate `InputMapper` logic in the
|
||||
remote.
|
||||
|
||||
## Robot architecture (`robot/`, Teensy 4.0)
|
||||
## Robot architecture (`robot/`, Teensy 3.6)
|
||||
|
||||
Loop at `CONTROL_HZ` (60): comms → InputMapper → IMU → heading integrate →
|
||||
StateMachine → gait/sit-stand targets → BodyPose → Limits clamp → IK →
|
||||
@@ -96,7 +97,7 @@ joint-rate clamp → servos.
|
||||
| `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 |
|
||||
| `ServoDriver` | 18 servos on hardware PWM (50 Hz, 16-bit), per-servo calibration from `Config.h` `SERVOS`, duty 0 = relaxed, skips invalid IK solutions |
|
||||
|
||||
## Remote architecture (`remote/`, Teensy 4.1)
|
||||
|
||||
@@ -215,15 +216,28 @@ Robot (`robot/README.md` has detail):
|
||||
(`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.
|
||||
- **Servos (owner-approved):**
|
||||
- The robot board is a **Teensy 3.6** (owner confirmed), and
|
||||
`platformio.ini` targets `teensy36`.
|
||||
- `Config.h` `SERVOS[6][3]` is a per-servo table: pin, `us_at_0deg`,
|
||||
signed `us_per_deg` (the sign is the direction), and `us_min`/`us_max`.
|
||||
It's built by `oldServo()` from the old sketch's numbers (range, trim,
|
||||
zero, direction, offset) so they stay readable.
|
||||
- `ServoDriver` drives all 18 servos with hardware PWM: 50 Hz, 16-bit
|
||||
`analogWrite`, and duty 0 = relaxed. The Teensy 3.6 `Servo` library
|
||||
stops at 12 servos, which is why the old sketch used PWMServo.
|
||||
- Radio: CE 39 / CSN 15, with SPI SCK moved to pin 14
|
||||
(`SPI.setSCK(RADIO_SCK_PIN)` in `Comms::begin`).
|
||||
- The sim's shims implement `analogWrite`/`analogWriteFrequency`/
|
||||
`analogWriteResolution`, and decode pulses through the same table (API
|
||||
version 6).
|
||||
- The old ±90° offsets on the rear-left/front-left coxas left those
|
||||
servos at the end of their travel, and the sim showed those legs
|
||||
clamping (8 failing checks). The owner chose to **re-horn both by 90°**,
|
||||
so the offsets are 0. Must be done on the robot before running this
|
||||
firmware.
|
||||
- BENCH: every direction, plus the femur's ≈6.7 µs/° (vs ≈10.4 for
|
||||
coxa/tibia).
|
||||
- IK knee direction (`tibia_rad` sign), BodyPose roll/pitch sign, IMU
|
||||
register map, and the sit pose assuming feet stay grounded.
|
||||
- **Fixed after the owner saw them in Unity (sim_smoke now guards each):**
|
||||
@@ -278,6 +292,34 @@ Robot (`robot/README.md` has detail):
|
||||
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.
|
||||
- **Old robot sketch (`LEGACY/Hexapod_Teensy/`)**: real hardware facts.
|
||||
It received 18 servo angles (0–180) over RF24Network (channel 90) from
|
||||
the old remote/Unity and wrote them with PWMServo. No IK, no gait.
|
||||
- **Board:** Teensy 3.6 (pins 36–39 and `SPI.setSCK(14)`, confirmed by
|
||||
the owner).
|
||||
- **Radio:** CE 39, CSN 15, SPI SCK 14 / MOSI 11 / MISO 12. Now in
|
||||
`robot/Config.h`.
|
||||
- **Servo pins** (servo index 0–17 = 6 × coxa, femur, tibia):
|
||||
`4,3,2, 7,6,5, 10,9,8, 23,22,21, 20,36,37, 17,16,38`. No 11/12
|
||||
conflict with SPI.
|
||||
- **Index → leg** (from the old Unity `Leg.startServo`):
|
||||
- 0 = rear-right, 3 = mid-right, 6 = front-right (firmware legs 2, 1, 0);
|
||||
- 9 = rear-left, 12 = mid-left, 15 = front-left (legs 3, 4, 5).
|
||||
- **Pulse ranges** (PWMServo maps 0–180° linearly onto min..max):
|
||||
- coxa and tibia 553–2425 µs (≈10.4 µs/°);
|
||||
- femur 840–2050 µs (≈6.7 µs/°);
|
||||
- plus a per-servo µs trim `servoCallibration[]`
|
||||
(`0,100,0, 0,100,-30, 0,80,-50, -120,0,0, -10,20,0, 0,20,-80`).
|
||||
- **Angle offsets:** `servoOffset[]` is +90° on index 9 (RL coxa) and
|
||||
−90° on index 15 (FL coxa), so those two horns are mounted rotated.
|
||||
The owner will re-horn them; the new firmware uses offset 0.
|
||||
- **Direction mapping** (derived from the old Unity `Leg.cs`, in this
|
||||
repo's joint convention):
|
||||
- coxa servo = 90 − coxa;
|
||||
- femur servo = 90 + femur (right), 90 − femur (left);
|
||||
- tibia servo = −tibia (right), 180 + tibia (left), so 90 = knee bent
|
||||
90°.
|
||||
- All of this is now in `Config.h` `SERVOS` (see "Servos" above).
|
||||
- `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
|
||||
|
||||
Reference in new issue
Block a user