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>

2026-current/robot/Config.h

@@ -44,19 +44,62 @@ constexpr float STANCE_HEIGHT_RATE_MM_S = 60.0f;
 // it. BENCH: set to roughly the width of a leg (servo body + bracket).
 constexpr float LEG_CLEARANCE_MM = 30.0f;
 
-// ---- Servo pins (Teensy 4.0). Verify these are PWM-capable on your board. ----
-// Order per leg: coxa, femur, tibia.
-constexpr uint8_t SERVO_PINS[NUM_LEGS][3] = {
-  {  2,  3,  4 },   // leg 0
-  {  5,  6,  7 },   // leg 1
-  {  8,  9, 10 },   // leg 2
-  { 11, 12, 24 },   // leg 3
-  { 25, 26, 27 },   // leg 4
-  { 28, 29, 30 },   // leg 5
+// ---- Servos (Teensy 3.6, hardware PWM at SERVO_PWM_HZ) ----
+// One entry per servo: pulse = us_at_0deg + us_per_deg * joint_deg, clamped
+// to us_min..us_max. us_per_deg is signed - its sign is the servo's direction.
+// Joint angles are this firmware's convention (see Kinematics.cpp): coxa
+// + = CCW from the mount angle, femur + = up, tibia relative to femur, + = up.
+struct ServoCal {
+  uint8_t pin;
+  float us_at_0deg;
+  float us_per_deg;
+  int16_t us_min, us_max;
 };
-// Microseconds at 0deg and per additional degree. Calibrate per servo - they vary.
-constexpr int SERVO_US_AT_0DEG = 1500;
-constexpr int SERVO_US_PER_DEG = 10;
+
+// Built from the old robot sketch (LEGACY/Hexapod_Teensy/config.h), which
+// drove each servo with PWMServo: a command of 0..180 maps linearly onto
+// min..max microseconds (servoMin/servoMax + servoCallibration trim). The
+// command for a joint angle is  zero + dir * joint_deg + offset, where
+// zero/dir come from the old Unity Leg.cs servo mapping and offset is the
+// old servoOffset[]:
+//   coxa   90 - coxa              (both sides)
+//   femur  90 + femur  right,  90 - femur  left
+//   tibia  0 - tibia   right,  180 + tibia left   (90 = knee bent 90 deg)
+constexpr ServoCal oldServo(uint8_t pin, int min, int max, int trim, float zero, int dir, float offset) {
+  return { pin,
+           (min + trim) + (max - min) / 180.0f * (zero + offset),
+           dir * (max - min) / 180.0f,
+           (int16_t)(min + trim), (int16_t)(max + trim) };
+}
+
+// Old ranges: coxa and tibia 553..2425 us, femur 840..2050 us over the 0..180
+// command. Comments give the old servo index (old Unity Leg.startServo).
+// BENCH:
+//  - The femur's narrower range means ~6.7 us per command degree vs ~10.4
+//    for coxa/tibia. Check one femur moves the angle the sim shows.
+//  - The old code had +90 / -90 coxa offsets on rear-left / front-left,
+//    which put those two servos at the end of their travel at rest (command
+//    180 / 0), so they could only swing one way (the simulation showed
+//    those legs clamping as soon as the robot turned or posed). Owner's
+//    decision: re-mount those two horns 90 deg so the leg points straight
+//    out at servo center, like the others - offsets are 0 here. Do that
+//    before powering the robot with this firmware.
+//  - Check every direction on the bench, one joint at a time.
+constexpr ServoCal SERVOS[NUM_LEGS][3] = {
+  // leg 0 front-right (old 6-8)
+  { oldServo(10, 553, 2425,    0, 90, -1,   0), oldServo( 9, 840, 2050, 80, 90, +1, 0), oldServo( 8, 553, 2425, -50,   0, -1, 0) },
+  // leg 1 mid-right (old 3-5)
+  { oldServo( 7, 553, 2425,    0, 90, -1,   0), oldServo( 6, 840, 2050, 100, 90, +1, 0), oldServo( 5, 553, 2425, -30,   0, -1, 0) },
+  // leg 2 rear-right (old 0-2)
+  { oldServo( 4, 553, 2425,    0, 90, -1,   0), oldServo( 3, 840, 2050, 100, 90, +1, 0), oldServo( 2, 553, 2425,   0,   0, -1, 0) },
+  // leg 3 rear-left (old 9-11). Coxa offset was +90 in the old code: horn to be re-mounted (see above).
+  { oldServo(23, 553, 2425, -120, 90, -1,   0), oldServo(22, 840, 2050,   0, 90, -1, 0), oldServo(21, 553, 2425,   0, 180, +1, 0) },
+  // leg 4 mid-left (old 12-14)
+  { oldServo(20, 553, 2425,  -10, 90, -1,   0), oldServo(36, 840, 2050,  20, 90, -1, 0), oldServo(37, 553, 2425,   0, 180, +1, 0) },
+  // leg 5 front-left (old 15-17). Coxa offset was -90 in the old code: horn to be re-mounted (see above).
+  { oldServo(17, 553, 2425,    0, 90, -1,   0), oldServo(16, 840, 2050,  20, 90, -1, 0), oldServo(38, 553, 2425, -80, 180, +1, 0) },
+};
+constexpr float SERVO_PWM_HZ = 50.0f;
 
 // ---- Gait timing ----
 constexpr float DEFAULT_CYCLE_SEC = 0.8f;   // one full gait cycle at neutral speed
@@ -68,8 +111,12 @@ constexpr float CONTROL_HZ = 60.0f;
 // ---- Radio (nRF24L01) ----
 // Address/channel/data rate/PA/payload size live in shared/HexapodProtocol
 // so the robot and remote can't drift apart. Only this board's wiring is here.
-constexpr uint8_t RADIO_CE_PIN     = 33;
-constexpr uint8_t RADIO_CSN_PIN    = 34;
+// From the old robot sketch (LEGACY/Hexapod_Teensy/Nrf.h): SPI on MOSI 11 /
+// MISO 12 with SCK moved to its alternate pin 14 (Teensy 3.x), so pin 13
+// stays free.
+constexpr uint8_t RADIO_CE_PIN     = 39;
+constexpr uint8_t RADIO_CSN_PIN    = 15;
+constexpr uint8_t RADIO_SCK_PIN    = 14;
 constexpr float   LINK_TIMEOUT_SEC = 0.3f;
 
 // ---- Robot-gyro self-leveling ----