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-10-01 15:53:55 +02:00
1 parent a961fa721b
commit 5ff85c4eb8
@@ -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
@@ -38,7 +38,6 @@ namespace HexapodSim
        public float stanceRadiusMm, stanceHeightMm;
        public float coxaMinDeg, coxaMaxDeg, femurMinDeg, femurMaxDeg, tibiaMinDeg, tibiaMaxDeg;
        public float controlHz;
-        public int servoUsAt0Deg, servoUsPerDeg;
        public float legClearanceMm;
        public float stanceHeightRangeMm;
    }
@@ -72,7 +71,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 = 5;   // HXSIM_API_VERSION
+        public const int ExpectedApiVersion = 6;   // HXSIM_API_VERSION

        [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate int IntFn();
        [UnmanagedFunctionPointer(CallingConvention.Cdecl)] delegate void VoidFn();
@@ -0,0 +1,93 @@
+#include "Nrf.h"
+#include <Wire.h>
+#include <MPU6050_tockn.h>
+#include "PWMServo.h"
+#include "config.h"
+
+PWMServo servos[18];
+
+MPU6050 mpu (Wire);
+
+dataStruct2 data;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(115200);
+  //  while (!Serial) ; // wait for arduino serial monitor
+
+  for (int i = 0; i < 18; i++)
+    servos[i].attach(pins[i], servoMin[i] + servoCallibration[i],
+                     servoMax[i] + servoCallibration[i]); // 900-2100 for high torque servos
+//  while (true)
+//  {
+    for (int i = 0; i < 18; i++)
+      servos[i].write(90 + servoOffset[i]);
+//  }
+
+
+  Serial.println("Starting mpu6050");
+
+//  //   Initialization
+//  Wire.begin();
+//  mpu.begin();
+//
+//  mpu.calcGyroOffsets(true, 300, 1000);
+//
+//  // Calibration
+//  Serial.println("=====================================");
+//  Serial.println("Starting calibration...");
+//  mpu.calcGyroOffsets();
+//  Serial.println("Calibration complete!");
+
+  Serial.println("Starting nrf");
+  NrfInit();
+}
+
+void STOP()
+{
+  for (int i = 0; i < 18; i++)
+    pinMode(pins[i], INPUT);
+  while (true)
+  {}
+}
+
+int i = 0;
+
+void loop()
+{
+  network.update();
+
+  if (false)
+  {
+    mpu.update();
+    Serial.print("AngX = ");
+    Serial.print(mpu.getAngleX());
+    Serial.print("  /  AngY = ");
+    Serial.print(mpu.getAngleY());
+    Serial.print("  /  AngZ = ");
+    Serial.println(mpu.getAngleZ());
+  }
+
+  //    Serial.print("  /  AngX = ");
+  //    Serial.print(mpu.getGyroAngleX());
+  //    Serial.print("  /  AngY = ");
+  //    Serial.print(mpu.getGyroAngleY());
+  //    Serial.print("  /  AngZ = ");
+  //    Serial.println(mpu.getGyroAngleZ());
+
+  data = NrfGetData(data, true);
+
+//  if (data.STOP)
+//    STOP();
+
+  // test
+  //  data.servoPositions[0] = data.servoPositions[0] + 0.1f;
+  //  data = NrfSendData(data, false);
+
+  if (data.remotePasstroughControl == true)
+    for (int i = 0; i < 18; i++)
+      servos[i].write(data.servoPositions[i] + servoOffset[i]);
+
+  delay(20);
+}
@@ -0,0 +1,66 @@
+#include <SPI.h>
+#include "RF24.h"
+#include <RF24Network.h>
+#include "TransferDataStructure.h"
+
+// Nrf24 settings
+RF24 radio(39, 15); // CE, CSN
+RF24Network network(radio);
+
+const uint16_t this_node = 01;        // Address of our node in Octal format
+const uint16_t other_node = 00;       // Address of the other node in Octal format
+
+void NrfInit()
+{
+  SPI.setSCK(14);
+  SPI.setMOSI(11);
+  SPI.setMISO(12);
+
+  SPI.begin();
+  radio.begin();
+  network.begin(/*channel*/ 90, /*node address*/ this_node);
+
+  // Set the PA Level low to prevent power supply related issues since this is a
+  // getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
+  radio.setPALevel(RF24_PA_LOW);
+}
+
+dataStruct2 NrfGetData(dataStruct2 data, bool debug = false)
+{
+  //  network.update();
+  int timeout = millis() + 100;
+  while ( network.available() && timeout > millis() )
+  {
+    RF24NetworkHeader header;
+    network.read(header, &data, sizeof(data));
+
+    if (debug)
+    {
+      Serial.print("Received packet ");
+      Serial.println(data.servoPositions[0]);
+    }
+  }
+
+  return data;
+}
+
+dataStruct2 NrfSendData(dataStruct2 data, bool debug = false)
+{
+  //  network.update();
+
+  if (debug)
+    Serial.print("Sending...");
+
+  RF24NetworkHeader header(/*to node*/ other_node);
+  bool ok = network.write(header, &data, sizeof(data));
+
+  if (debug)
+  {
+    if (ok)
+      Serial.println("ok.");
+    else
+      Serial.println("failed.");
+  }
+
+  return data;
+}
@@ -0,0 +1,226 @@
+#include "Arduino.h"
+#include "PWMServo.h"
+
+/*
+  PWMServo.cpp - Hardware Servo Timer Library
+  http://arduiniana.org/libraries/pwmservo/
+  Author: Jim Studt, jim@federated.com
+  Copyright (c) 2007 David A. Mellis.  All right reserved.
+  renamed to PWMServo by Mikal Hart
+  ported to other chips by Paul Stoffregen
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#define NO_ANGLE (0xff)
+
+#if defined(__AVR__)
+
+#include <avr/interrupt.h>
+
+uint8_t PWMServo::attachedA = 0;
+uint8_t PWMServo::attachedB = 0;
+#ifdef SERVO_PIN_C
+uint8_t PWMServo::attachedC = 0;
+#endif
+
+void PWMServo::seizeTimer1()
+{
+  uint8_t oldSREG = SREG;
+
+  cli();
+  TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
+  TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
+           | _BV(CS11) /* div 8 clock prescaler */
+           ;
+  OCR1A = 3000;
+  OCR1B = 3000;
+  ICR1 = clockCyclesPerMicrosecond() * (20000L / 8); // 20000 uS is a bit fast for the refresh, 20ms, but
+  // it keeps us from overflowing ICR1 at 20MHz clocks
+  // That "/8" at the end is the prescaler.
+#if defined(__AVR_ATmega8__)
+  TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
+#else
+  TIMSK1 &=  ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
+#endif
+
+  SREG = oldSREG;  // undo cli()
+}
+
+void PWMServo::releaseTimer1() {}
+
+PWMServo::PWMServo() : pin(0), angle(NO_ANGLE) {}
+
+uint8_t PWMServo::attach(int pinArg, int min, int max)
+{
+#ifdef SERVO_PIN_C
+  if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B && pinArg != SERVO_PIN_C) return 0;
+#else
+  if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B) return 0;
+#endif
+
+  min16 = min / 16;
+  max16 = max / 16;
+
+  pin = pinArg;
+  angle = NO_ANGLE;
+  digitalWrite(pin, LOW);
+  pinMode(pin, OUTPUT);
+
+#ifdef SERVO_PIN_C
+  if (!attachedA && !attachedB && !attachedC) seizeTimer1();
+#else
+  if (!attachedA && !attachedB) seizeTimer1();
+#endif
+
+  if (pin == SERVO_PIN_A) {
+    attachedA = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1);
+  }
+
+  if (pin == SERVO_PIN_B) {
+    attachedB = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1);
+  }
+
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) {
+    attachedC = 1;
+    TCCR1A = (TCCR1A & ~_BV(COM1C0)) | _BV(COM1C1);
+  }
+#endif
+  return 1;
+}
+
+void PWMServo::detach()
+{
+  // muck with timer flags
+  if (pin == SERVO_PIN_A) {
+    attachedA = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1);
+    pinMode(pin, INPUT);
+  }
+
+  if (pin == SERVO_PIN_B) {
+    attachedB = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1);
+    pinMode(pin, INPUT);
+  }
+
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) {
+    attachedC = 0;
+    TCCR1A = TCCR1A & ~_BV(COM1C0) & ~_BV(COM1C1);
+    pinMode(pin, INPUT);
+  }
+#endif
+
+#ifdef SERVO_PIN_C
+  if (!attachedA && !attachedB && !attachedC) releaseTimer1();
+#else
+  if (!attachedA && !attachedB) releaseTimer1();
+#endif
+}
+
+void PWMServo::write(int angleArg)
+{
+  uint16_t p;
+
+  if (angleArg < 0) angleArg = 0;
+  if (angleArg > 180) angleArg = 180;
+  angle = angleArg;
+
+  // bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
+  // That 8L on the end is the TCNT1 prescaler, it will need to change if the clock's prescaler changes,
+  // but then there will likely be an overflow problem, so it will have to be handled by a human.
+  p = (min16 * 16L * clockCyclesPerMicrosecond() + (max16 - min16) * (16L * clockCyclesPerMicrosecond()) * angle / 180L) / 8L;
+  if (pin == SERVO_PIN_A) OCR1A = p;
+  if (pin == SERVO_PIN_B) OCR1B = p;
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C) OCR1C = p;
+#endif
+}
+
+uint8_t PWMServo::attached()
+{
+  if (pin == SERVO_PIN_A && attachedA) return 1;
+  if (pin == SERVO_PIN_B && attachedB) return 1;
+#ifdef SERVO_PIN_C
+  if (pin == SERVO_PIN_C && attachedC) return 1;
+#endif
+  return 0;
+}
+
+
+#elif defined(__arm__) && defined(TEENSYDUINO)
+
+uint32_t PWMServo::attachedpins[(NUM_DIGITAL_PINS + 31) / 32]; // 1 bit per digital pin
+
+PWMServo::PWMServo() : pin(255), angle(NO_ANGLE) {}
+
+uint8_t PWMServo::attach(int pinArg, int min, int max)
+{
+//  Serial.printf("attach, pin=%d, min=%d, max=%d\n", pinArg, min, max);
+  if (pinArg < 0 || pinArg >= NUM_DIGITAL_PINS) return 0;
+  if (!digitalPinHasPWM(pinArg)) return 0;
+  pin = pinArg;
+  analogWriteFrequency(pin, 50);
+  min16 = min >> 4;
+  max16 = max >> 4;
+  angle = NO_ANGLE;
+  digitalWrite(pin, LOW);
+  pinMode(pin, OUTPUT);
+  attachedpins[pin >> 5] |= (1 << (pin & 31));
+//  Serial.println("Succes!");
+  return 1;
+}
+
+void PWMServo::write(float angleArg)
+{
+  //Serial.printf("write, pin=%d, angle=%d\n", pin, angleArg);
+  if (pin >= NUM_DIGITAL_PINS) return;
+  if (angleArg < 0) angleArg = 0;
+  if (angleArg > 180) angleArg = 180;
+  angle = angleArg;
+
+  uint32_t us = (((max16 - min16) * 46603 * (uint8_t)angle) >> 11) + (min16 << 12); // us*256
+  //uint32_t duty = (us * 3355) >> 22;
+
+  float usec = (float)((max16 - min16) << 4) * ((float)angle / 180.0f) + (float)(min16 << 4);
+  uint32_t duty = (int)(usec / 20000.0f * 4096.0f);
+
+  if (false)
+    Serial.printf("pin=%d, angle=%.2f, usec=%.2f, us=%.2f, duty=%d, min=%d, max=%d\n",
+                  pin, (float)angle, usec, (float)us / 256.0f, duty, min16 << 4, max16 << 4);
+
+#if TEENSYDUINO >= 137
+  noInterrupts();
+  uint32_t oldres = analogWriteResolution(12);
+  analogWrite(pin, duty);
+  analogWriteResolution(oldres);
+  interrupts();
+#else
+  analogWriteResolution(12);
+  analogWrite(pin, duty);
+#endif
+}
+
+uint8_t PWMServo::attached()
+{
+  if (pin >= NUM_DIGITAL_PINS) return 0;
+  return (attachedpins[pin >> 5] & (1 << (pin & 31))) ? 1 : 0;
+}
+
+#endif
@@ -0,0 +1,94 @@
+#ifndef PWMServo_h
+#define PWMServo_h
+
+/*
+  PWMServo.h - Hardware Servo Timer Library
+  http://arduiniana.org/libraries/pwmservo/
+  Author: Jim Studt, jim@federated.com
+  Copyright (c) 2007 David A. Mellis.  All right reserved.
+  renamed to PWMServo by Mikal Hart
+  ported to other chips by Paul Stoffregen
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <inttypes.h>
+
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) // Arduino
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // Arduino Mega
+  #define SERVO_PIN_A 11
+  #define SERVO_PIN_B 12
+  #define SERVO_PIN_C 13
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino
+  #define SERVO_PIN_A 13
+  #define SERVO_PIN_B 12
+#elif defined(__AVR_AT90USB162__) // Teensy 1.0
+  #define SERVO_PIN_A 17
+  #define SERVO_PIN_B 18
+  #define SERVO_PIN_C 15
+#elif defined(__AVR_ATmega32U4__) // Teensy 2.0
+  #define SERVO_PIN_A 14
+  #define SERVO_PIN_B 15
+  #define SERVO_PIN_C 4
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) // Teensy++
+  #define SERVO_PIN_A 25
+  #define SERVO_PIN_B 26
+  #define SERVO_PIN_C 27
+#elif defined(__arm__) && defined(TEENSYDUINO)
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+  // all PWM pins supported, but names not defined here...
+  // just use the actual PWM pin number with attach()
+#else
+  #define SERVO_PIN_A 9
+  #define SERVO_PIN_B 10
+#endif
+
+class PWMServo
+{
+  private:
+    uint8_t pin;
+    float angle;       // in degrees
+    uint8_t min16;       // minimum pulse, 16uS units  (default is 34)
+    uint8_t max16;       // maximum pulse, 16uS units, 0-4ms range (default is 150)
+#if defined(__AVR__)
+    static void seizeTimer1();
+    static void releaseTimer1();
+    static uint8_t attachedA;
+    static uint8_t attachedB;
+    #ifdef SERVO_PIN_C
+    static uint8_t attachedC;
+    #endif
+#elif defined(__arm__) && defined(TEENSYDUINO)
+    static uint32_t attachedpins[]; // 1 bit per digital pin
+#endif
+  public:
+    PWMServo();
+    uint8_t attach(int pinArg) { return attach(pinArg, 544, 2400); }
+                             // pulse length for 0 degrees in microseconds, 544uS default
+                             // pulse length for 180 degrees in microseconds, 2400uS default
+    uint8_t attach(int pinArg, int min, int max);
+                             // attach to a pin, sets pinMode, returns 0 on failure, won't
+                             // position the servo until a subsequent write() happens
+                             // Only works for 9 and 10.
+    void detach();
+    void write(float angleArg); // specify the angle in degrees, 0 to 180
+    uint8_t read() { return angle; }
+    uint8_t attached();
+};
+
+#endif
@@ -0,0 +1,29 @@
+#ifndef dataStruct
+#define dataStruct
+
+struct __attribute__((__packed__)) dataStruct2
+{
+  bool STOP = false;
+
+  float walkSpeed = 10; //step size
+  float walkSpeed2 = 0; //gate speed
+  float walkRotation = 0 ;
+  float walkDirection = 0;
+
+  float tranform[3] = {0, 0, 0};
+  float tranform2[3] = {0, 0, 0};
+  float rotation[3] = {0, 0, 0};
+
+  bool remotePasstroughControl = true;
+  float servoPositions[18] = {
+    90, 90, 90,
+    90, 90, 90,
+    90, 90, 90,
+
+    90, 90, 90,
+    90, 90, 90,
+    90, 90, 90
+  };
+} dataStruct;
+
+#endif
@@ -0,0 +1,44 @@
+byte pins[18] = {4, 3, 2,
+                 7, 6, 5,
+                 10, 9, 8,
+
+                 23, 22, 21,
+                 20, 36, 37,
+                 17, 16, 38
+                };
+
+int servoMin[18] = {553, 840, 553,
+                    553, 840, 553,
+                    553, 840, 553,
+
+                    553, 840, 553,
+                    553, 840, 553,
+                    553, 840, 553
+                   };
+
+int servoMax[18] = {2425, 2050, 2425,
+                    2425, 2050, 2425,
+                    2425, 2050, 2425,
+
+                    2425, 2050, 2425,
+                    2425, 2050, 2425,
+                    2425, 2050, 2425
+                   };
+
+int servoCallibration[18] = {0, 100, 0,
+                             0, 100, -30,
+                             0, 80, -50,
+
+                             -120, 0, 0,
+                             -10, 20, 0,
+                             0, 20, -80
+                            };
+
+float servoOffset[18] = {0, 0, 0,
+                         0, 0, 0,
+                         0, 0, 0,
+
+                         90, 0, 0,
+                         0, 0, 0,
+                         -90, 0, 0
+                        };
@@ -1,6 +1,6 @@
 # Hexapod

-A six-legged walking robot (Teensy 4.0) and its handheld remote
+A six-legged walking robot (Teensy 3.6) and its handheld remote
 (Teensy 4.1 + Nextion touchscreen), linked over nRF24L01.

 ```
@@ -1,5 +1,5 @@
 ; Hexapod: two firmware images in one project.
-;   pio run -e robot            build robot/  (Teensy 4.0)
+;   pio run -e robot            build robot/  (Teensy 3.6)
 ;   pio run -e remote           build remote/ (Teensy 4.1)
 ;   pio run -e robot -t upload  flash (Teensy Loader / teensy_cli)
 ;   pio device monitor -e robot serial monitor at 115200
@@ -21,7 +21,7 @@ lib_deps =
    nrf24/RF24@^1.4.9

 [env:robot]
-board = teensy40
+board = teensy36
 custom_src_dir = robot

 [env:remote]
@@ -9,6 +9,7 @@ static_assert(HXP_RADIO_DATA_RATE == RF24_250KBPS && HXP_RADIO_PA_LEVEL == RF24_
 static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);

 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
@@ -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 ----
@@ -1,4 +1,4 @@
-# Hexapod robot firmware (Teensy 4.0)
+# Hexapod robot firmware (Teensy 3.6)

 This is the **robot-side** firmware. It is fully functional on its own
 (driven by the serial test harness below) and receives commands from the
@@ -10,8 +10,8 @@ sketches compile against.

 From the repo root (PlatformIO): `pio run -e robot`, flash with
 `pio run -e robot -t upload`, monitor with `pio device monitor -e robot`.
-RF24 is pulled in automatically via `lib_deps`; Servo ships with the Teensy
-framework.
+RF24 is pulled in automatically via `lib_deps`. Servos are driven by the
+Teensy's hardware PWM directly, so no servo library is needed.

 Arduino IDE also works if you copy `shared/HexapodProtocol/` into your
 sketchbook `libraries/` folder and install **RF24** by TMRh20.
@@ -31,15 +31,18 @@ still a placeholder. Check all of it against the real robot:
 - `LEG_CLEARANCE_MM`: how close neighbouring legs may get. Walking speed
  and body pose are scaled down to keep it, so set it to about one leg's
  width.
- `SERVO_PINS` — must be PWM-capable pins on your Teensy 4.0 wiring
- `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` — calibrate per servo, they vary
-  batch to batch
- `RADIO_CE_PIN` / `RADIO_CSN_PIN` — your nRF24 wiring. Address, channel,
-  data rate, PA level and payload size are in `shared/HexapodProtocol` so
-  they can't drift from the remote.
- **Pin conflict:** `SERVO_PINS` leg 3 uses pins 11 and 12, which are the
-  Teensy 4.0's SPI MOSI/MISO that the nRF24 needs. Move those two servos
-  before wiring the radio.
+- `SERVOS`: one entry per servo with pin, pulse at 0°, signed µs per
+  degree (sign = direction) and pulse limits. It's prefilled from the old
+  robot sketch (`LEGACY/Hexapod_Teensy/config.h`): pins, ranges, trims, and
+  directions from the old Unity servo mapping. Check every joint's
+  direction on the bench.
+- **Before powering up:** re-mount the rear-left and front-left coxa servo
+  horns 90° so those legs point straight out at servo center. The old code
+  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.

 ## Bench-check checklist (do this before the first stand-up)

@@ -69,12 +72,12 @@ for your build:
   each foot opposite to the commanded motion. The smoke test checks that
   `+vx` moves the body forward and `+yaw_rate` turns it left (CCW). On
   the bench, confirm `w` walks forward and `z` turns left.
-6. **Per-servo calibration**. `ServoDriver` uses one
-   `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG` for all 18 servos, so servo
-   center = joint angle 0 (tibia *straight*). On this build the knee servo
-   center is the tibia at 90° to the femur, and the left side is mirrored
-   (per the old Unity/servo code). That needs a per-joint offset and
-   direction before the real robot will stand.
+6. **Per-servo calibration** (`Config.h` `SERVOS`). Arm the robot,
+   stand it on a box, and move one joint at a time (or watch the sim next
+   to it): each servo must turn the way the sim shows. To flip a direction,
+   flip that entry's `dir`. Also check that a femur moves the commanded
+   angle: its old range gives only ≈6.7 µs/° against ≈10.4 for coxa and
+   tibia.

 ## Testing in the simulation

@@ -1,28 +1,42 @@
 #include "ServoDriver.h"
 #include "Config.h"
-#include <Servo.h>

-static Servo servos[NUM_LEGS][3];
+// 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
+// step. Every servo pin must be PWM-capable - on the Teensy 3.6 all of the
+// SERVOS[] pins are.
+static constexpr uint8_t PWM_BITS = 16;
+static constexpr float PERIOD_US = 1e6f / SERVO_PWM_HZ;
+
+static void writePulse(uint8_t pin, float us) {
+  analogWrite(pin, (int)(us / PERIOD_US * (1 << PWM_BITS) + 0.5f));
+}

 void ServoDriver::begin() {
+  analogWriteResolution(PWM_BITS);
  for (int i = 0; i < NUM_LEGS; i++)
-    for (int j = 0; j < 3; j++)
-      servos[i][j].attach(SERVO_PINS[i][j]);
+    for (int j = 0; j < 3; j++) {
+      analogWriteFrequency(SERVOS[i][j].pin, SERVO_PWM_HZ);
+      pinMode(SERVOS[i][j].pin, OUTPUT);
+      analogWrite(SERVOS[i][j].pin, 0);   // no pulses until the first writeLeg()
+    }
 }

 void ServoDriver::relaxAll() {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++)
-      servos[i][j].detach();
+      analogWrite(SERVOS[i][j].pin, 0);   // no pulses: the servos go limp
 }

-static int degToMicros(float deg) {
-  return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG);
+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);
 }

 void ServoDriver::writeLeg(int i, const JointAngles& a) {
  if (!a.valid) return;   // never write an out-of-range or unreachable solution
-  servos[i][0].writeMicroseconds(degToMicros(a.coxa_deg));
-  servos[i][1].writeMicroseconds(degToMicros(a.femur_deg));
-  servos[i][2].writeMicroseconds(degToMicros(a.tibia_deg));
+  writePulse(SERVOS[i][0].pin, degToMicros(i, 0, a.coxa_deg));
+  writePulse(SERVOS[i][1].pin, degToMicros(i, 1, a.femur_deg));
+  writePulse(SERVOS[i][2].pin, degToMicros(i, 2, a.tibia_deg));
 }
@@ -3,7 +3,10 @@

 class ServoDriver {
 public:
-  void begin();                                       // attach all servos
-  void relaxAll();                                     // detach all servos - call when disarming
+  void begin();                                       // set up the PWM pins (no pulses yet)
+  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).
+  static int degToMicros(int leg, int joint, float deg);
 };
@@ -50,8 +50,6 @@ HXSIM_API void hxsim_get_geometry(HxGeometry* g) {
  g->femur_min_deg = FEMUR_MIN_DEG; g->femur_max_deg = FEMUR_MAX_DEG;
  g->tibia_min_deg = TIBIA_MIN_DEG; g->tibia_max_deg = TIBIA_MAX_DEG;
  g->control_hz = CONTROL_HZ;
-  g->servo_us_at_0deg = SERVO_US_AT_0DEG;
-  g->servo_us_per_deg = SERVO_US_PER_DEG;
  g->leg_clearance_mm = LEG_CLEARANCE_MM;
  g->stance_height_range_mm = STANCE_HEIGHT_RANGE_MM;
 }
@@ -105,22 +103,24 @@ HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap) { return simhw::se
 HXSIM_API void hxsim_get_servos(int32_t* us, int32_t* attached, uint32_t* writes) {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++) {
-      const simhw::ServoPin& p = simhw::servoPin(SERVO_PINS[i][j]);
+      const simhw::ServoPin& p = simhw::servoPin(SERVOS[i][j].pin);
      if (us)       us[i * 3 + j] = p.us;
      if (attached) attached[i * 3 + j] = p.attached ? 1 : 0;
      if (writes)   writes[i * 3 + j] = p.writes;
    }
 }

-// Inverse of ServoDriver::degToMicros - what angle the firmware meant.
-static float pulseToDeg(int us) {
-  return (us - SERVO_US_AT_0DEG) / (float)SERVO_US_PER_DEG;
+// Inverse of ServoDriver::degToMicros through the same per-servo table:
+// what angle the firmware meant by this pulse.
+static float pulseToDeg(int leg, int joint, int us) {
+  const ServoCal& c = SERVOS[leg][joint];
+  return (us - c.us_at_0deg) / c.us_per_deg;
 }

 HXSIM_API void hxsim_get_joints_deg(float* deg) {
  for (int i = 0; i < NUM_LEGS; i++)
    for (int j = 0; j < 3; j++)
-      deg[i * 3 + j] = pulseToDeg(simhw::servoPin(SERVO_PINS[i][j]).us);
+      deg[i * 3 + j] = pulseToDeg(i, j, simhw::servoPin(SERVOS[i][j].pin).us);
 }

 // Forward kinematics, written independently of robot/Kinematics.cpp so it
@@ -18,7 +18,7 @@
 #endif

 // Bump whenever a signature or struct below changes; the C# side checks it.
-#define HXSIM_API_VERSION 5
+#define HXSIM_API_VERSION 6

 #define HXSIM_NUM_LEGS 6

@@ -34,8 +34,6 @@ typedef struct {
  float femur_min_deg, femur_max_deg;
  float tibia_min_deg, tibia_max_deg;
  float control_hz;
-  int32_t servo_us_at_0deg;
-  int32_t servo_us_per_deg;
  float leg_clearance_mm;    // LEG_CLEARANCE_MM: minimum gap between neighbouring legs' segments
  float stance_height_range_mm;   // CmdPacket.body_height +-1000 moves the height by this much
 } HxGeometry;
@@ -81,7 +79,7 @@ HXSIM_API void    hxsim_serial_input(const char* bytes, int32_t len);    // TEST
 // Outputs
 HXSIM_API int32_t hxsim_serial_output(char* out, int32_t cap);           // drains robot Serial prints
 HXSIM_API void    hxsim_get_servos(int32_t* us_18, int32_t* attached_18, uint32_t* writes_18);
-HXSIM_API void    hxsim_get_joints_deg(float* deg_18);   // servo pulses decoded with Config.h constants
+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);
 // Kinematic body (SimBody.cpp): the body rests on the support plane of its
@@ -7,7 +7,7 @@ simulation is exactly the code you flash to the Teensy. `robot/` has no
 `#ifdef SIM` anywhere.

 ```
-robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 4.0 firmware
+robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 3.6 firmware
                   └─► CMake + g++ + sim/shim/  ──► sim/build/hexapod_sim.dll ──► Unity
 ```

@@ -17,7 +17,7 @@ robot/ (unchanged) ──► PlatformIO + Teensy core ──► Teensy 4.0 firmw

 | Firmware uses | Simulated as |
 |---|---|
-| `Servo::writeMicroseconds()` | the pulse stored per pin, decoded back to angles with `Config.h`'s `SERVO_US_*` |
+| `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 |
 | `millis()` / `micros()` / `Serial` | a simulated clock that Unity advances, and captured serial I/O |
@@ -114,7 +114,9 @@ component to the scene yourself.
 ## What is not simulated

 - Physics (mass, falling, slipping, servo torque and speed).
- Servo calibration. The sim decodes pulses with `Config.h`'s single
-  `SERVO_US_AT_0DEG` / `SERVO_US_PER_DEG`, i.e. what the firmware *intends*,
-  not per-servo offsets or directions.
+- Whether the calibration matches the real servos. The sim decodes pulses
+  through the same `SERVOS` table the firmware uses, so it shows what the
+  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.
@@ -4,6 +4,7 @@
 #include <Servo.h>
 #include <Wire.h>
 #include <RF24.h>
+#include <SPI.h>
 #include <EEPROM.h>
 #include <HexapodProtocol.h>
 #include <stdarg.h>
@@ -13,6 +14,7 @@

 SimSerial Serial;
 TwoWire Wire;
+SPIClass SPI;
 SimEEPROM EEPROM;

 // =====================================================================
@@ -103,6 +105,36 @@ void Servo::writeMicroseconds(int us) {
  g_pins[pin_].writes++;
 }

+// ---- Hardware PWM (how ServoDriver drives the servos on the Teensy 3.6) ----
+static uint32_t g_pwm_bits = 8;                 // Teensyduino default
+static float g_pwm_hz[simhw::MAX_PINS];         // 0 = not set (Teensy default ~488 Hz)
+
+void pinMode(uint8_t pin, uint8_t mode) {
+  if (pin < simhw::MAX_PINS && mode == INPUT) g_pins[pin].attached = false;
+}
+void digitalWrite(uint8_t, uint8_t) {}
+uint32_t analogWriteResolution(uint32_t bits) {
+  uint32_t old = g_pwm_bits;
+  g_pwm_bits = bits;
+  return old;
+}
+void analogWriteFrequency(uint8_t pin, float hz) {
+  if (pin < simhw::MAX_PINS) g_pwm_hz[pin] = hz;
+}
+// A duty of 0 means no pulses (a relaxed servo); otherwise the pulse width
+// follows from duty, resolution and frequency, exactly as on the hardware.
+void analogWrite(uint8_t pin, int value) {
+  if (pin >= simhw::MAX_PINS) return;
+  if (value <= 0) {
+    g_pins[pin].attached = false;
+    return;
+  }
+  float hz = g_pwm_hz[pin] > 0 ? g_pwm_hz[pin] : 488.28f;
+  g_pins[pin].us = (int)lroundf((float)value / (1u << g_pwm_bits) * (1e6f / hz));
+  g_pins[pin].attached = true;
+  g_pins[pin].writes++;
+}
+
 // =====================================================================
 // I2C + MPU6050 (register-level, address 0x68)
 // =====================================================================
@@ -28,6 +28,18 @@ constexpr typename std::common_type<A, B>::type max(A a, B b) { return a > b ? a
 inline double radians(double deg) { return deg * (PI / 180.0); }
 inline double degrees(double rad) { return rad * (180.0 / PI); }

+// Pins and hardware PWM. analogWrite() pulses are recorded per pin, so the
+// simulation sees servo pulses exactly as the PWM hardware would make them.
+#define INPUT 0
+#define OUTPUT 1
+#define LOW 0
+#define HIGH 1
+void pinMode(uint8_t pin, uint8_t mode);
+void digitalWrite(uint8_t pin, uint8_t value);
+void analogWrite(uint8_t pin, int value);
+uint32_t analogWriteResolution(uint32_t bits);   // returns the previous resolution, like Teensyduino
+void analogWriteFrequency(uint8_t pin, float hz);
+
 // Simulated clock, advanced by the simulation (see SimHardware.cpp).
 uint32_t millis();
 uint32_t micros();
@@ -1,2 +1,12 @@
 #pragma once
-// Nothing needed: the simulated RF24 doesn't talk SPI.
+#include <Arduino.h>
+
+// The simulated RF24 doesn't talk SPI; this only has to accept the pin setup.
+class SPIClass {
+public:
+  void begin() {}
+  void setSCK(uint8_t) {}
+  void setMOSI(uint8_t) {}
+  void setMISO(uint8_t) {}
+};
+extern SPIClass SPI;