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/ServoDriver.cpp

@@ -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));
 }