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

@@ -0,0 +1,65 @@
+#include "GaitEngine.h"
+
+const GaitPattern GAIT_TRIPOD = {
+  0.5f,
+  { 0.5f, 0.0f, 0.5f, 0.0f, 0.5f, 0.0f }
+};
+
+const GaitPattern GAIT_WAVE = {
+  5.0f / 6.0f,
+  { 0.0f, 1.0f / 6, 2.0f / 6, 3.0f / 6, 4.0f / 6, 5.0f / 6 }
+};
+
+void GaitEngine::begin() {
+  for (int i = 0; i < 6; i++) {
+    legs_pattern_[i] = requested_pattern_;
+    leg_phase_[i] = legs_pattern_[i]->leg_phase_offset[i];
+  }
+}
+
+static float smoothstep(float t) { return t * t * (3.0f - 2.0f * t); }
+
+void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) {
+  for (int i = 0; i < 6; i++) {
+    leg_phase_[i] += dt / cycle_sec_;
+
+    // Touchdown: this leg's cycle wraps back to 0 - the one moment it's safe
+    // to hand it a newly requested pattern, since it's grounded with zero
+    // relative velocity right here regardless of which two patterns are involved.
+    if (leg_phase_[i] >= 1.0f) {
+      leg_phase_[i] -= 1.0f;
+      legs_pattern_[i] = requested_pattern_;
+    }
+
+    const GaitPattern* p = legs_pattern_[i];
+    bool in_stance = leg_phase_[i] < p->duty_factor;
+
+    FootTarget neutral = Kinematics::neutralStance(i);
+    float stride_time = cycle_sec_ * p->duty_factor;
+
+    // Half-stride displacement from the commanded twist.
+    float half_len_x = twist.vx * stride_time * 0.5f;
+    float half_len_y = twist.vy * stride_time * 0.5f;
+    // Rough yaw contribution using this leg's mount position as the moment arm.
+    float yaw_dx = -twist.omega * LEG_MOUNT_Y_MM[i] * stride_time * 0.5f;
+    float yaw_dy =  twist.omega * LEG_MOUNT_X_MM[i] * stride_time * 0.5f;
+
+    FootTarget start = { neutral.x - half_len_x - yaw_dx, neutral.y - half_len_y - yaw_dy, neutral.z };
+    FootTarget end    = { neutral.x + half_len_x + yaw_dx, neutral.y + half_len_y + yaw_dy, neutral.z };
+
+    FootTarget foot;
+    if (in_stance) {
+      float t = leg_phase_[i] / p->duty_factor;
+      foot.x = start.x + (end.x - start.x) * t;
+      foot.y = start.y + (end.y - start.y) * t;
+      foot.z = neutral.z;
+    } else {
+      float t = (leg_phase_[i] - p->duty_factor) / (1.0f - p->duty_factor);
+      float st = smoothstep(t);
+      foot.x = end.x + (start.x - end.x) * st;
+      foot.y = end.y + (start.y - end.y) * st;
+      foot.z = neutral.z + SWING_LIFT_MM * sin(PI * t);
+    }
+    out[i] = foot;
+  }
+}