Initial import: robot firmware + PlatformIO two-env scaffold

Robot-side firmware (Teensy 4.0) imported unchanged as extracted.
platformio.ini defines [env:robot] (teensy40) and [env:remote]
(teensy41); scripts/select_src_dir.py points each env at its own
sketch folder so robot/ and remote/ stay Arduino-style sketches.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

2026-current/robot/ServoDriver.cpp

@@ -0,0 +1,28 @@
+#include "ServoDriver.h"
+#include "Config.h"
+#include <Servo.h>
+
+static Servo servos[NUM_LEGS][3];
+
+void ServoDriver::begin() {
+  for (int i = 0; i < NUM_LEGS; i++)
+    for (int j = 0; j < 3; j++)
+      servos[i][j].attach(SERVO_PINS[i][j]);
+}
+
+void ServoDriver::relaxAll() {
+  for (int i = 0; i < NUM_LEGS; i++)
+    for (int j = 0; j < 3; j++)
+      servos[i][j].detach();
+}
+
+static int degToMicros(float deg) {
+  return SERVO_US_AT_0DEG + (int)(deg * SERVO_US_PER_DEG);
+}
+
+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));
+}