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/.gitignore

@@ -0,0 +1,28 @@
+# PlatformIO
+.pio/
+.pioenvs/
+.piolibdeps/
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch/
+
+# Arduino IDE / arduino-cli build output
+build/
+*.elf
+*.hex
+*.bin
+*.map
+*.o
+*.d
+
+# Nextion Editor build output (keep the .HMI source, ignore compiled .tft)
+*.tft
+
+# OS / editor noise
+.DS_Store
+Thumbs.db
+desktop.ini
+*.swp
+*~
+__pycache__/

2026-current/Hexapod.code-workspace

@@ -0,0 +1,7 @@
+{
+	"folders": [
+		{
+			"path": "."
+		}
+	]
+}

2026-current/platformio.ini

@@ -0,0 +1,29 @@
+; Hexapod: two firmware images in one project.
+;   pio run -e robot            build robot/  (Teensy 4.0)
+;   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
+;
+; shared/ is the library directory. Both sketches #include <HexapodProtocol.h>
+; from shared/HexapodProtocol/, which is the single definition of the wire
+; protocol (CmdPacket, FLAG_* bits, radio address/channel/rate/PA/payload size).
+
+[platformio]
+default_envs = robot, remote
+lib_dir = shared
+
+[env]
+platform = teensy
+framework = arduino
+monitor_speed = 115200
+extra_scripts = pre:scripts/select_src_dir.py
+lib_deps =
+    nrf24/RF24@^1.4.9
+
+[env:robot]
+board = teensy40
+custom_src_dir = robot
+
+[env:remote]
+board = teensy41
+custom_src_dir = remote

2026-current/robot/BodyPose.cpp

@@ -0,0 +1,50 @@
+#include "BodyPose.h"
+
+void BodyPose::begin() {
+  s_roll_.reset(0); s_pitch_.reset(0); s_yaw_.reset(0);
+  s_x_.reset(0); s_y_.reset(0); s_z_.reset(0);
+}
+
+void BodyPose::update(float dt) {
+  s_roll_.setTarget(command_.roll_deg);
+  s_pitch_.setTarget(command_.pitch_deg);
+  s_yaw_.setTarget(command_.yaw_deg);
+  s_x_.setTarget(command_.x_mm);
+  s_y_.setTarget(command_.y_mm);
+  s_z_.setTarget(command_.z_mm);
+
+  s_roll_.update(dt); s_pitch_.update(dt); s_yaw_.update(dt);
+  s_x_.update(dt); s_y_.update(dt); s_z_.update(dt);
+}
+
+// Combines the commanded (smoothed) tilt with the robot's own stabilization
+// correction, then applies both to a body-frame foot target so the feet stay
+// planted while the body tilts/shifts on top of them.
+//
+// Sign convention here (whether positive roll tips the body left or right,
+// whether offsetting +X moves the feet the way you expect) depends on your
+// leg numbering and mount angles - bench-check it the same way as the IK
+// elbow direction, and flip signs if it goes the wrong way.
+FootTarget BodyPose::apply(const FootTarget& in) const {
+  float roll  = radians(s_roll_.current + correction_roll_);
+  float pitch = radians(s_pitch_.current + correction_pitch_);
+  float yaw   = radians(s_yaw_.current);
+
+  float x = in.x, y = in.y, z = in.z;
+
+  // yaw (about Z)
+  float x1 = x * cos(yaw) - y * sin(yaw);
+  float y1 = x * sin(yaw) + y * cos(yaw);
+  // pitch (about Y)
+  float x2 =  x1 * cos(pitch) + z * sin(pitch);
+  float z2 = -x1 * sin(pitch) + z * cos(pitch);
+  // roll (about X)
+  float y3 = y1 * cos(roll) - z2 * sin(roll);
+  float z3 = y1 * sin(roll) + z2 * cos(roll);
+
+  FootTarget out;
+  out.x = x2 - s_x_.current;
+  out.y = y3 - s_y_.current;
+  out.z = z3 - s_z_.current;
+  return out;
+}

2026-current/robot/BodyPose.h

@@ -0,0 +1,36 @@
+#pragma once
+#include "SmoothedValue.h"
+#include "Kinematics.h"
+#include "Config.h"
+
+struct PoseCommand {
+  float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
+  float x_mm = 0, y_mm = 0, z_mm = 0;
+};
+
+class BodyPose {
+public:
+  void begin();
+  void setCommand(const PoseCommand& c) { command_ = c; }
+  // From RobotIMU stabilization; pass 0,0 when the robot-gyro toggle is off.
+  void setCorrection(float roll_deg, float pitch_deg) {
+    correction_roll_ = roll_deg;
+    correction_pitch_ = pitch_deg;
+  }
+
+  void update(float dt);
+
+  // Applies the current (smoothed) pose transform to a body-frame foot target.
+  FootTarget apply(const FootTarget& in) const;
+
+private:
+  PoseCommand command_;
+  float correction_roll_ = 0, correction_pitch_ = 0;
+
+  SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_yaw_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_x_{POSE_OFFSET_MAX_RATE_MM};
+  SmoothedValue s_y_{POSE_OFFSET_MAX_RATE_MM};
+  SmoothedValue s_z_{POSE_OFFSET_MAX_RATE_MM};
+};

2026-current/robot/Comms.cpp

@@ -0,0 +1,28 @@
+#include "Comms.h"
+#include "Config.h"
+#include <RF24.h>
+#include <SPI.h>
+
+static RF24 radio(RADIO_CE_PIN, RADIO_CSN_PIN);
+
+bool Comms::begin() {
+  radio_present_ = radio.begin();
+  if (radio_present_) {
+    radio.openReadingPipe(1, RADIO_ADDRESS);
+    radio.setPALevel(RF24_PA_LOW);
+    radio.setDataRate(RF24_250KBPS);
+    radio.setChannel(96);          // pick one clear of your local Wi-Fi
+    radio.startListening();
+  }
+  return radio_present_;
+}
+
+void Comms::update() {
+  if (radio_present_) {
+    while (radio.available()) {
+      radio.read(&packet_, sizeof(packet_));
+      last_rx_ms_ = millis();
+    }
+  }
+  link_ok_ = (millis() - last_rx_ms_) < (uint32_t)(LINK_TIMEOUT_SEC * 1000);
+}

2026-current/robot/Comms.h

@@ -0,0 +1,40 @@
+#pragma once
+#include <Arduino.h>
+
+// Remote -> robot, sent at a fixed rate. Kept as the single source of truth
+// here so a future remote sketch just has to fill this same struct and
+// radio.write() it - nothing on the robot side needs to change.
+struct CmdPacket {
+  int16_t vx = 0, vy = 0;           // left stick: strafe / forward-back, -1000..1000
+  int16_t yaw_rate = 0;             // left stick twist, -1000..1000
+  int16_t rx = 0, ry = 0, rz = 0;   // right stick: roll/pitch/yaw OR X/Y/height, per flags
+  uint8_t speed_scale = 128;        // 0-255
+  uint8_t flags = 0;                // bit-packed, see FLAG_* below
+  uint8_t seq = 0;
+};
+
+constexpr uint8_t FLAG_ARMED        = 1 << 0;
+constexpr uint8_t FLAG_STAND        = 1 << 1;   // 0 = sit requested, 1 = stand requested
+constexpr uint8_t FLAG_WALK         = 1 << 2;   // stick past deadzone -> walking
+constexpr uint8_t FLAG_MODE_B       = 1 << 3;   // 0 = mode A direct, 1 = mode B heading-hold
+constexpr uint8_t FLAG_RSTICK_XLATE = 1 << 4;   // 0 = right stick = rotation, 1 = translation
+constexpr uint8_t FLAG_GAIT_WAVE    = 1 << 5;   // 0 = tripod requested, 1 = wave requested
+constexpr uint8_t FLAG_REMOTE_GYRO  = 1 << 6;   // remote tilt feedforward enabled
+constexpr uint8_t FLAG_ROBOT_GYRO   = 1 << 7;   // robot stabilization feedback enabled
+
+class Comms {
+public:
+  bool begin();
+  void update();                       // call every loop; polls the radio if present
+  bool linkOk() const { return link_ok_; }
+  const CmdPacket& latest() const { return packet_; }
+
+  // Lets the serial test harness (or, later, real radio) inject a packet directly.
+  void injectPacket(const CmdPacket& p) { packet_ = p; last_rx_ms_ = millis(); }
+
+private:
+  CmdPacket packet_;
+  uint32_t last_rx_ms_ = 0;
+  bool link_ok_ = false;
+  bool radio_present_ = false;
+};

2026-current/robot/Config.h

@@ -0,0 +1,59 @@
+#pragma once
+#include <Arduino.h>
+
+// =====================================================================
+// Everything in this file is a placeholder for a typical small hexapod.
+// Measure your own robot and edit these before trusting it on the floor.
+// =====================================================================
+
+// ---- Leg geometry ----
+constexpr int NUM_LEGS = 6;
+// Leg order: 0=front-right, 1=mid-right, 2=rear-right, 3=rear-left, 4=mid-left, 5=front-left
+// Angles measured counterclockwise from the body's +X (forward) axis, viewed from above.
+constexpr float LEG_MOUNT_ANGLE_DEG[NUM_LEGS] = { -45, -90, -135, 135, 90, 45 };
+constexpr float LEG_MOUNT_X_MM[NUM_LEGS]      = {  80,   0,  -80,  -80,   0,  80 };
+constexpr float LEG_MOUNT_Y_MM[NUM_LEGS]      = { -60, -90,  -60,   60,  90,  60 };
+
+constexpr float COXA_LEN_MM  = 30.0f;
+constexpr float FEMUR_LEN_MM = 60.0f;
+constexpr float TIBIA_LEN_MM = 100.0f;
+
+// ---- Joint limits, degrees ----
+constexpr float COXA_MIN_DEG  = -60,  COXA_MAX_DEG  = 60;
+constexpr float FEMUR_MIN_DEG = -80,  FEMUR_MAX_DEG = 80;
+constexpr float TIBIA_MIN_DEG = -150, TIBIA_MAX_DEG = -20;
+
+// ---- Neutral stance, relative to each leg's own coxa pivot ----
+constexpr float STANCE_RADIUS_MM = 140.0f;  // how far out the foot rests
+constexpr float STANCE_HEIGHT_MM = 90.0f;   // how far below the coxa pivot the foot rests
+
+// ---- 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
+};
+// 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;
+
+// ---- Gait timing ----
+constexpr float DEFAULT_CYCLE_SEC = 0.8f;   // one full gait cycle at neutral speed
+constexpr float SWING_LIFT_MM     = 35.0f;
+
+// ---- Control loop ----
+constexpr float CONTROL_HZ = 60.0f;
+
+// ---- Radio (nRF24L01) ----
+constexpr uint8_t RADIO_CE_PIN     = 33;
+constexpr uint8_t RADIO_CSN_PIN    = 34;
+constexpr uint8_t RADIO_ADDRESS[6] = "HXPD1";   // must match the remote once it exists
+constexpr float   LINK_TIMEOUT_SEC = 0.3f;
+
+// ---- Setpoint smoothing rates ----
+constexpr float POSE_TILT_MAX_RATE_DEG  = 120.0f;  // roll/pitch/yaw offset, deg/s
+constexpr float POSE_OFFSET_MAX_RATE_MM = 150.0f;  // X/Y/Z offset, mm/s

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;
+  }
+}

2026-current/robot/GaitEngine.h

@@ -0,0 +1,35 @@
+#pragma once
+#include "Kinematics.h"
+#include "Config.h"
+
+struct GaitPattern {
+  float duty_factor;              // fraction of the cycle each leg spends in stance
+  float leg_phase_offset[6];      // each leg's phase offset within the cycle, 0..1
+};
+
+extern const GaitPattern GAIT_TRIPOD;
+extern const GaitPattern GAIT_WAVE;
+
+struct Twist {
+  float vx = 0, vy = 0;   // mm/s, body frame
+  float omega = 0;        // rad/s, body frame
+};
+
+class GaitEngine {
+public:
+  void begin();
+  void setRequestedPattern(const GaitPattern* p) { requested_pattern_ = p; }
+  void setCycleDuration(float sec) { cycle_sec_ = sec; }
+
+  // Advances every leg by dt under the current twist and pattern, writing each
+  // leg's target foot position (body frame) into out[6].
+  void update(float dt, const Twist& twist, FootTarget out[6]);
+
+  bool legInStance(int i) const { return leg_phase_[i] < legs_pattern_[i]->duty_factor; }
+
+private:
+  float leg_phase_[6];
+  const GaitPattern* legs_pattern_[6];
+  const GaitPattern* requested_pattern_ = &GAIT_TRIPOD;
+  float cycle_sec_ = DEFAULT_CYCLE_SEC;
+};

2026-current/robot/InputMapper.cpp

@@ -0,0 +1,60 @@
+#include "InputMapper.h"
+#include <Arduino.h>
+
+static float normalize(int16_t raw) { return constrain(raw, -1000, 1000) / 1000.0f; }
+
+static float wrapDeg(float d) {
+  while (d > 180) d -= 360;
+  while (d < -180) d += 360;
+  return d;
+}
+
+MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
+  (void)dt;
+  MapperOutput out{};
+  out.armed               = p.flags & FLAG_ARMED;
+  out.stand_request        = p.flags & FLAG_STAND;
+  out.walk_active           = p.flags & FLAG_WALK;
+  out.mode_b                = p.flags & FLAG_MODE_B;
+  out.gait_wave              = p.flags & FLAG_GAIT_WAVE;
+  out.remote_gyro_enabled     = p.flags & FLAG_REMOTE_GYRO;
+  out.robot_gyro_enabled       = p.flags & FLAG_ROBOT_GYRO;
+
+  float speed = p.speed_scale / 255.0f;
+  float raw_vx = normalize(p.vx) * speed;
+  float raw_vy = normalize(p.vy) * speed;
+  float raw_yaw_in = normalize(p.yaw_rate);
+
+  if (!out.mode_b) {
+    // Mode A: direct twist. Heading and travel direction are fully independent.
+    out.twist.vx = raw_vx * MAX_LINEAR_MM_S;
+    out.twist.vy = raw_vy * MAX_LINEAR_MM_S;
+    out.twist.omega = radians(raw_yaw_in * MAX_TURN_RATE_DEG_S);
+  } else {
+    // Mode B: heading-hold. Stick angle becomes a target heading; forward
+    // speed comes from stick magnitude. yaw_rate is unused here.
+    float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
+    if (mag > 0.05f) {
+      heading_target_deg_ = degrees(atan2(raw_vy, raw_vx));
+    }
+    float heading_error = wrapDeg(heading_target_deg_ - heading_deg);
+    float omega_deg_s = constrain(HEADING_KP * heading_error, -MAX_TURN_RATE_DEG_S, MAX_TURN_RATE_DEG_S);
+    out.twist.omega = radians(omega_deg_s);
+    out.twist.vx = mag * MAX_LINEAR_MM_S;   // forward-only in the robot's current facing
+    out.twist.vy = 0;
+  }
+
+  // Right stick: rotation sub-mode (default) or translation sub-mode.
+  float rx = normalize(p.rx), ry = normalize(p.ry), rz = normalize(p.rz);
+  if (p.flags & FLAG_RSTICK_XLATE) {
+    out.pose.x_mm = rx * MAX_POSE_OFFSET_MM;
+    out.pose.y_mm = ry * MAX_POSE_OFFSET_MM;
+    out.pose.z_mm = rz * MAX_POSE_OFFSET_MM;
+  } else {
+    out.pose.roll_deg  = rx * MAX_TILT_DEG;
+    out.pose.pitch_deg = ry * MAX_TILT_DEG;
+    out.pose.yaw_deg   = rz * MAX_TILT_DEG;
+  }
+
+  return out;
+}

2026-current/robot/InputMapper.h

@@ -0,0 +1,30 @@
+#pragma once
+#include "Comms.h"
+#include "GaitEngine.h"
+#include "BodyPose.h"
+
+struct MapperOutput {
+  Twist twist;
+  PoseCommand pose;
+  bool armed = false;
+  bool stand_request = false;
+  bool walk_active = false;
+  bool mode_b = false;
+  bool gait_wave = false;
+  bool remote_gyro_enabled = false;
+  bool robot_gyro_enabled = false;
+};
+
+class InputMapper {
+public:
+  // heading_deg is the robot's current tracked heading estimate; only mode B uses it.
+  MapperOutput map(const CmdPacket& p, float heading_deg, float dt);
+
+private:
+  float heading_target_deg_ = 0;
+  static constexpr float MAX_TURN_RATE_DEG_S = 90.0f;
+  static constexpr float HEADING_KP = 2.0f;
+  static constexpr float MAX_LINEAR_MM_S = 150.0f;   // tune to your leg workspace / servo speed
+  static constexpr float MAX_POSE_OFFSET_MM = 40.0f;
+  static constexpr float MAX_TILT_DEG = 20.0f;
+};

2026-current/robot/Kinematics.cpp

@@ -0,0 +1,67 @@
+#include "Kinematics.h"
+#include "Config.h"
+
+// Standard analytical 3-DOF leg IK: coxa handles yaw in the horizontal plane,
+// femur/tibia form a 2-link planar problem solved with the law of cosines.
+//
+// The elbow direction (which way tibia_rad's sign comes out) and the overall
+// roll/pitch sign convention both depend on which way your leg is physically
+// mounted - bench-check this on one leg with known foot positions before
+// trusting all six.
+JointAngles Kinematics::solve(int i, const FootTarget& target) {
+  JointAngles out{};
+
+  // Body-frame target -> leg-local frame: translate to the coxa pivot, then
+  // rotate by -mount angle so the leg's local +X points along its resting
+  // outward direction.
+  float dx = target.x - LEG_MOUNT_X_MM[i];
+  float dy = target.y - LEG_MOUNT_Y_MM[i];
+  float a  = radians(-LEG_MOUNT_ANGLE_DEG[i]);
+  float local_x = dx * cos(a) - dy * sin(a);
+  float local_y = dx * sin(a) + dy * cos(a);
+  float local_z = target.z;
+
+  float coxa_rad = atan2(local_y, local_x);
+
+  // Horizontal distance from the femur pivot (after the coxa segment) to the foot.
+  float horiz = sqrt(local_x * local_x + local_y * local_y) - COXA_LEN_MM;
+  float dist  = sqrt(horiz * horiz + local_z * local_z);
+
+  out.valid = true;
+  float max_reach = FEMUR_LEN_MM + TIBIA_LEN_MM;
+  float min_reach = fabs(FEMUR_LEN_MM - TIBIA_LEN_MM);
+  if (dist > max_reach) { dist = max_reach * 0.999f; out.valid = false; }
+  if (dist < min_reach) { dist = min_reach * 1.001f; out.valid = false; }
+
+  // Law of cosines: knee (tibia) angle from the femur/tibia/foot-distance triangle.
+  float cos_tibia = (FEMUR_LEN_MM * FEMUR_LEN_MM + TIBIA_LEN_MM * TIBIA_LEN_MM - dist * dist)
+                     / (2.0f * FEMUR_LEN_MM * TIBIA_LEN_MM);
+  cos_tibia = constrain(cos_tibia, -1.0f, 1.0f);
+  float tibia_rad = -(PI - acos(cos_tibia));   // knee-down convention; flip sign if yours mounts the other way
+
+  // Femur angle: elevation to the foot, plus the triangle's femur-side angle.
+  float elevation = atan2(-local_z, horiz);
+  float cos_femur_tri = (FEMUR_LEN_MM * FEMUR_LEN_MM + dist * dist - TIBIA_LEN_MM * TIBIA_LEN_MM)
+                         / (2.0f * FEMUR_LEN_MM * dist);
+  cos_femur_tri = constrain(cos_femur_tri, -1.0f, 1.0f);
+  float femur_rad = elevation + acos(cos_femur_tri);
+
+  out.coxa_deg  = degrees(coxa_rad);
+  out.femur_deg = degrees(femur_rad);
+  out.tibia_deg = degrees(tibia_rad);
+
+  if (out.coxa_deg  < COXA_MIN_DEG  || out.coxa_deg  > COXA_MAX_DEG)  out.valid = false;
+  if (out.femur_deg < FEMUR_MIN_DEG || out.femur_deg > FEMUR_MAX_DEG) out.valid = false;
+  if (out.tibia_deg < TIBIA_MIN_DEG || out.tibia_deg > TIBIA_MAX_DEG) out.valid = false;
+
+  return out;
+}
+
+FootTarget Kinematics::neutralStance(int i) {
+  float a = radians(LEG_MOUNT_ANGLE_DEG[i]);
+  FootTarget t;
+  t.x = LEG_MOUNT_X_MM[i] + cos(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
+  t.y = LEG_MOUNT_Y_MM[i] + sin(a) * (COXA_LEN_MM + STANCE_RADIUS_MM);
+  t.z = -STANCE_HEIGHT_MM;
+  return t;
+}

2026-current/robot/Kinematics.h

@@ -0,0 +1,22 @@
+#pragma once
+#include <Arduino.h>
+
+struct JointAngles {
+  float coxa_deg;
+  float femur_deg;
+  float tibia_deg;
+  bool valid;   // false if the target was unreachable or outside joint limits
+};
+
+struct FootTarget {
+  float x, y, z;   // body-frame mm, relative to body center
+};
+
+class Kinematics {
+public:
+  // Solves leg i's joint angles for a body-frame foot target.
+  static JointAngles solve(int leg_index, const FootTarget& target);
+
+  // The leg's resting/neutral foot target, in body frame.
+  static FootTarget neutralStance(int leg_index);
+};

2026-current/robot/Limits.cpp

@@ -0,0 +1,35 @@
+#include "Limits.h"
+#include <Arduino.h>
+#include <math.h>
+
+FootTarget Limits::clampWorkspace(int i, const FootTarget& target) {
+  FootTarget neutral = Kinematics::neutralStance(i);
+  float dx = target.x - neutral.x;
+  float dy = target.y - neutral.y;
+  float dz = target.z - neutral.z;
+
+  constexpr float MAX_HORIZ_MM = 70.0f;   // tune to your real leg reach, with margin
+  constexpr float MAX_VERT_MM  = 60.0f;
+
+  float horiz = sqrt(dx * dx + dy * dy);
+  if (horiz > MAX_HORIZ_MM) {
+    float scale = MAX_HORIZ_MM / horiz;
+    dx *= scale;
+    dy *= scale;
+  }
+  dz = constrain(dz, -MAX_VERT_MM, MAX_VERT_MM);
+
+  return { neutral.x + dx, neutral.y + dy, neutral.z + dz };
+}
+
+float Limits::clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt) {
+  float max_step = max_deg_per_sec * dt;
+  float delta = constrain(target_deg - current_deg, -max_step, max_step);
+  return current_deg + delta;
+}
+
+bool Limits::stancePolygonOk(const bool in_stance[6]) {
+  int count = 0;
+  for (int i = 0; i < 6; i++) if (in_stance[i]) count++;
+  return count >= 3;
+}

2026-current/robot/Limits.h

@@ -0,0 +1,15 @@
+#pragma once
+#include "Kinematics.h"
+
+class Limits {
+public:
+  // Clamps a foot target into a conservative safe envelope around the leg's neutral stance.
+  static FootTarget clampWorkspace(int leg_index, const FootTarget& target);
+
+  // Rate-limits a joint angle change to the servo's real max speed.
+  static float clampJointRate(float current_deg, float target_deg, float max_deg_per_sec, float dt);
+
+  // Coarse safety check: are enough legs grounded right now to be stable.
+  // A real support-polygon/center-of-mass check is a good later upgrade.
+  static bool stancePolygonOk(const bool in_stance[6]);
+};

2026-current/robot/README.md

@@ -0,0 +1,113 @@
+# Hexapod robot firmware (Teensy 4.0)
+
+This is the **robot-side** firmware only. It is fully functional on its own
+(driven by the serial test harness below) and is already wired to receive
+commands from a future remote over nRF24L01 — the packet format in
+`Comms.h` is the contract the remote sketch must match.
+
+## Required libraries
+
+Install via Arduino Library Manager (or Teensyduino's manager):
+
+- **RF24** by TMRh20 — nRF24L01 radio driver
+- **Servo** — bundled with Teensyduino, nothing to install
+
+## Before you trust this on the floor
+
+Everything in `Config.h` is a placeholder for a generic small hexapod. Measure
+your own robot and edit:
+
+- `LEG_MOUNT_ANGLE_DEG` / `LEG_MOUNT_X_MM` / `LEG_MOUNT_Y_MM` — where each
+  coxa pivot actually sits on your chassis, and which way it points
+- `COXA_LEN_MM` / `FEMUR_LEN_MM` / `TIBIA_LEN_MM` — your link lengths
+- `COXA_MIN_DEG..TIBIA_MAX_DEG` — your servos' real safe range of motion
+- `STANCE_RADIUS_MM` / `STANCE_HEIGHT_MM` — how far out/down you want the
+  resting foot position
+- `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_ADDRESS` / channel (set in `Comms.cpp`) — must match the remote
+  once it exists
+
+## Bench-check checklist (do this before the first stand-up)
+
+These are sign conventions that depend on which way your hardware is
+physically mounted, and the code has a best-guess default that may be wrong
+for your build:
+
+1. **IK elbow/tibia direction** (`Kinematics.cpp`) — command one leg to a
+   few known foot positions by hand (e.g. via the serial harness) and watch
+   whether the knee bends the way you expect. Flip the sign noted in the
+   comment on `tibia_rad` if it bends backwards.
+2. **BodyPose roll/pitch sign** (`BodyPose.cpp`) — command a small roll/pitch
+   offset and verify the body leans the direction you asked for, not the
+   opposite.
+3. **RobotIMU register map** (`RobotIMU.cpp`) — written for an MPU6050-style
+   chip (I2C addr `0x68`). If your gyro is a different part, its register
+   addresses and scale factors will differ; swap them in.
+4. **Sit-pose assumption** — the built-in sit pose in `robot.ino` assumes all
+   six feet stay grounded and simply lowers the body (a 0.4x height blend).
+   If your chassis is meant to rest its belly on the ground instead, this
+   needs a different, sequenced approach (fold legs up rather than just
+   lowering).
+
+## Testing without a remote yet
+
+`robot.ino` has `#define TEST_SERIAL_INPUT` enabled by default. With that on,
+open the Arduino Serial Monitor (115200 baud, no line ending needed — single
+keystrokes) and use:
+
+| Key | Action |
+|-----|--------|
+| `a` | toggle armed |
+| `s` | toggle sit / stand |
+| `w` / `x` | walk forward / backward |
+| `q` / `e` | strafe left / right |
+| `z` / `c` | yaw left / right |
+| `g` | toggle gait: tripod ↔ wave |
+| `m` | toggle walk mode: direct (A) ↔ heading-hold (B) |
+| space | zero all velocity (stop walking, stay in current state) |
+
+Each keystroke injects a `CmdPacket` through `Comms::injectPacket()` — the
+exact same path a real remote's received radio packets go through, so the
+rest of the pipeline (mapper → state machine → gait → pose → IK → servos)
+runs identically to how it will with real hardware. Once you have a real
+remote transmitting, just remove the `#define TEST_SERIAL_INPUT` line (or
+leave it — injected packets simply won't happen if you don't type).
+
+Arm (`a`), then stand (`s`), before trying to walk — the state machine
+enforces `DISARMED → SIT → STAND → WALKING` in order and will otherwise
+ignore walk commands.
+
+## What's ready for the remote
+
+- `Comms.h`'s `CmdPacket` struct and `FLAG_*` bit constants are the exact
+  wire format the remote must send (as an RF24 payload to `RADIO_ADDRESS`
+  on the same channel/data-rate/PA-level set in `Comms::begin()`).
+- Failsafe: if no packet arrives for `LINK_TIMEOUT_SEC`, `Comms::linkOk()`
+  goes false and the main loop falls back to a zeroed `CmdPacket`, which
+  disarms the robot.
+- `FLAG_REMOTE_GYRO` / `FLAG_ROBOT_GYRO` independently gate whether the
+  remote's tilt feeds into body pose and whether the robot's own IMU
+  stabilizes it — matching the touchscreen toggles you described.
+- `FLAG_RSTICK_XLATE` switches the right stick between rotation-axes and
+  translation-axes interpretation, and `FLAG_MODE_B` switches between the
+  two walk modes, all inside the existing `InputMapper`.
+
+## File layout
+
+```
+robot/
+├── robot.ino          - main sketch: setup/loop, state wiring, serial test harness
+├── Config.h           - all placeholder constants (edit this first)
+├── SmoothedValue.h     - generic rate-limited setpoint smoother
+├── Kinematics.h/.cpp   - per-leg 3-DOF analytical IK
+├── GaitEngine.h/.cpp   - tripod/wave gait, per-leg phase, dynamic gait swap
+├── BodyPose.h/.cpp     - 6-axis body pose composition (feedforward + IMU correction)
+├── RobotIMU.h/.cpp     - gyro/accel read + complementary filter
+├── StateMachine.h/.cpp - DISARMED/SIT/STAND/WALKING with smoothed sit-stand blend
+├── InputMapper.h/.cpp  - CmdPacket -> twist/pose/mode decoding
+├── Limits.h/.cpp       - workspace clamp, joint rate limit, stance polygon check
+├── Comms.h/.cpp        - nRF24L01 receive + packet/flags definition
+└── ServoDriver.h/.cpp  - servo attach/relax/write
+```

2026-current/robot/RobotIMU.cpp

@@ -0,0 +1,51 @@
+#include "RobotIMU.h"
+#include <Wire.h>
+
+// MPU6050-style register map, default +/-2g / +/-250deg/s ranges.
+// Swap readRaw() if you're using a different IMU part.
+static constexpr uint8_t IMU_ADDR         = 0x68;
+static constexpr uint8_t REG_PWR_MGMT_1   = 0x6B;
+static constexpr uint8_t REG_ACCEL_XOUT_H = 0x3B;
+
+bool RobotIMU::begin() {
+  Wire.begin();
+  Wire.beginTransmission(IMU_ADDR);
+  Wire.write(REG_PWR_MGMT_1);
+  Wire.write(0x00);   // wake the IMU out of sleep
+  ok_ = (Wire.endTransmission() == 0);
+  return ok_;
+}
+
+bool RobotIMU::readRaw(float& ax, float& ay, float& az, float& gx, float& gy, float& gz) {
+  Wire.beginTransmission(IMU_ADDR);
+  Wire.write(REG_ACCEL_XOUT_H);
+  if (Wire.endTransmission(false) != 0) return false;
+  Wire.requestFrom((int)IMU_ADDR, 14);
+  if (Wire.available() < 14) return false;
+
+  int16_t raw_ax = (Wire.read() << 8) | Wire.read();
+  int16_t raw_ay = (Wire.read() << 8) | Wire.read();
+  int16_t raw_az = (Wire.read() << 8) | Wire.read();
+  Wire.read(); Wire.read();   // skip temperature
+  int16_t raw_gx = (Wire.read() << 8) | Wire.read();
+  int16_t raw_gy = (Wire.read() << 8) | Wire.read();
+  int16_t raw_gz = (Wire.read() << 8) | Wire.read();
+
+  ax = raw_ax / 16384.0f; ay = raw_ay / 16384.0f; az = raw_az / 16384.0f;
+  gx = raw_gx / 131.0f;   gy = raw_gy / 131.0f;   gz = raw_gz / 131.0f;   // deg/s
+  (void)gz;
+  return true;
+}
+
+void RobotIMU::update(float dt) {
+  if (!ok_ || !enabled) return;
+  float ax, ay, az, gx, gy, gz;
+  if (!readRaw(ax, ay, az, gx, gy, gz)) return;
+
+  float accel_roll  = degrees(atan2(ay, az));
+  float accel_pitch = degrees(atan2(-ax, sqrt(ay * ay + az * az)));
+
+  constexpr float ALPHA = 0.98f;   // trust the gyro short-term, the accelerometer long-term
+  roll_deg_  = ALPHA * (roll_deg_  + gx * dt) + (1 - ALPHA) * accel_roll;
+  pitch_deg_ = ALPHA * (pitch_deg_ + gy * dt) + (1 - ALPHA) * accel_pitch;
+}

2026-current/robot/RobotIMU.h

@@ -0,0 +1,18 @@
+#pragma once
+#include <Arduino.h>
+
+class RobotIMU {
+public:
+  bool begin();          // returns false if the IMU didn't respond
+  void update(float dt); // no-op if disabled or the IMU never came up
+  float rollDeg()  const { return roll_deg_; }
+  float pitchDeg() const { return pitch_deg_; }
+
+  bool enabled = true;   // robot-gyro toggle from CmdPacket sets this
+
+private:
+  float roll_deg_ = 0, pitch_deg_ = 0;
+  bool ok_ = false;
+
+  bool readRaw(float& ax, float& ay, float& az, float& gx, float& gy, float& gz);
+};

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

2026-current/robot/ServoDriver.h

@@ -0,0 +1,9 @@
+#pragma once
+#include "Kinematics.h"
+
+class ServoDriver {
+public:
+  void begin();                                       // attach all servos
+  void relaxAll();                                     // detach all servos - call when disarming
+  void writeLeg(int leg_index, const JointAngles& angles);
+};

2026-current/robot/SmoothedValue.h

@@ -0,0 +1,25 @@
+#pragma once
+
+// Rate-limited setpoint smoother. Every place a discrete command can jump
+// (body pose axes, gait speed, the sit/stand blend) gets one of these instead
+// of using the raw commanded value directly.
+struct SmoothedValue {
+  float current = 0;
+  float target  = 0;
+  float max_rate;   // units per second
+
+  explicit SmoothedValue(float rate = 100.0f) : max_rate(rate) {}
+
+  void setTarget(float t) { target = t; }
+
+  float update(float dt) {
+    float delta = target - current;
+    float max_step = max_rate * dt;
+    if (delta > max_step) delta = max_step;
+    else if (delta < -max_step) delta = -max_step;
+    current += delta;
+    return current;
+  }
+
+  void reset(float v) { current = target = v; }
+};

2026-current/robot/StateMachine.cpp

@@ -0,0 +1,22 @@
+#include "StateMachine.h"
+#include <Arduino.h>
+
+void StateMachine::update(bool armed, bool stand_request, bool walk_active, float dt) {
+  if (!armed) {
+    state_ = RobotState::DISARMED;
+    blend_ = 0;
+    return;
+  }
+  if (state_ == RobotState::DISARMED) {
+    state_ = RobotState::SIT;   // arming always lands in sit first, never straight to stand
+  }
+
+  float target_blend = stand_request ? 1.0f : 0.0f;
+  float step = BLEND_RATE * dt;
+  if (blend_ < target_blend)      blend_ = min(blend_ + step, target_blend);
+  else if (blend_ > target_blend) blend_ = max(blend_ - step, target_blend);
+
+  if (blend_ <= 0.001f)      state_ = RobotState::SIT;
+  else if (blend_ >= 0.999f) state_ = walk_active ? RobotState::WALKING : RobotState::STAND;
+  else                        state_ = RobotState::STAND;   // mid-transition
+}

2026-current/robot/StateMachine.h

@@ -0,0 +1,17 @@
+#pragma once
+
+enum class RobotState { DISARMED, SIT, STAND, WALKING };
+
+class StateMachine {
+public:
+  void begin() { state_ = RobotState::DISARMED; blend_ = 0; }
+  void update(bool armed_request, bool stand_request, bool walk_active_request, float dt);
+
+  RobotState state() const { return state_; }
+  float sitStandBlend() const { return blend_; }   // 0 = sit pose, 1 = stand pose
+
+private:
+  RobotState state_ = RobotState::DISARMED;
+  float blend_ = 0;
+  static constexpr float BLEND_RATE = 1.0f;   // per second - full sit->stand in ~1s
+};

2026-current/robot/robot.ino

@@ -0,0 +1,165 @@
+#include "Config.h"
+#include "Kinematics.h"
+#include "GaitEngine.h"
+#include "BodyPose.h"
+#include "RobotIMU.h"
+#include "StateMachine.h"
+#include "InputMapper.h"
+#include "Limits.h"
+#include "Comms.h"
+#include "ServoDriver.h"
+
+// Uncomment to drive the robot from the Serial Monitor before a remote exists.
+// See README.md for the key map.
+#define TEST_SERIAL_INPUT
+
+GaitEngine gait;
+BodyPose pose;
+RobotIMU imu;
+StateMachine state_machine;
+InputMapper mapper;
+Comms comms;
+ServoDriver servo_driver;
+
+float current_heading_deg = 0;          // open-loop: accumulates commanded omega. See note in loop().
+float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
+uint32_t last_loop_us = 0;
+RobotState last_state = RobotState::DISARMED;
+
+#ifdef TEST_SERIAL_INPUT
+CmdPacket test_packet;
+void pollSerialTestInput() {
+  bool changed = false;
+  while (Serial.available()) {
+    char c = Serial.read();
+    changed = true;
+    switch (c) {
+      case 'a': test_packet.flags ^= FLAG_ARMED; break;
+      case 's': test_packet.flags ^= FLAG_STAND; break;
+      case 'w': test_packet.vy = min(test_packet.vy + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      case 'x': test_packet.vy = max(test_packet.vy - 200, -1000); test_packet.flags |= FLAG_WALK; break;
+      case 'q': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break;
+      case 'e': test_packet.vx = min(test_packet.vx + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      case 'z': test_packet.yaw_rate = max(test_packet.yaw_rate - 200, -1000); test_packet.flags |= FLAG_WALK; break;
+      case 'c': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      case 'g': test_packet.flags ^= FLAG_GAIT_WAVE; break;
+      case 'm': test_packet.flags ^= FLAG_MODE_B; break;
+      case ' ':
+        test_packet.vx = 0; test_packet.vy = 0; test_packet.yaw_rate = 0;
+        test_packet.flags &= ~FLAG_WALK;
+        break;
+      default: changed = false; break;
+    }
+  }
+  if (changed) {
+    test_packet.speed_scale = 180;
+    comms.injectPacket(test_packet);
+  }
+}
+#endif
+
+void setup() {
+  Serial.begin(115200);
+  gait.begin();
+  pose.begin();
+  state_machine.begin();
+
+  if (!imu.begin())   Serial.println("robot IMU not responding - stabilization disabled");
+  if (!comms.begin()) Serial.println("radio not responding - waiting for injected/test commands only");
+
+  servo_driver.begin();
+  last_loop_us = micros();
+}
+
+void loop() {
+  uint32_t now_us = micros();
+  float dt = (now_us - last_loop_us) / 1e6f;
+  if (dt < (1.0f / CONTROL_HZ)) return;   // hold to the fixed control rate
+  last_loop_us = now_us;
+
+#ifdef TEST_SERIAL_INPUT
+  pollSerialTestInput();
+#endif
+
+  comms.update();
+  CmdPacket cmd = comms.linkOk() ? comms.latest() : CmdPacket();   // link lost -> all-neutral, unarmed
+
+  MapperOutput mapped = mapper.map(cmd, current_heading_deg, dt);
+
+  imu.enabled = mapped.robot_gyro_enabled;
+  imu.update(dt);
+
+  // Simple open-loop heading estimate: accumulates the omega we've been
+  // commanding. Good enough for mode B in a normal session; fusing the
+  // robot IMU's yaw rate in as well is the more robust upgrade if legs
+  // ever slip on your surface.
+  current_heading_deg += degrees(mapped.twist.omega) * dt;
+
+  state_machine.update(mapped.armed, mapped.stand_request, mapped.walk_active, dt);
+  RobotState state = state_machine.state();
+
+  // Relax servos on the way into DISARMED, reattach on the way out.
+  if (state == RobotState::DISARMED && last_state != RobotState::DISARMED) {
+    servo_driver.relaxAll();
+  } else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
+    servo_driver.begin();
+  }
+  last_state = state;
+
+  gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
+
+  // Remote-gyro toggle off -> drop commanded tilt but keep XYZ offset/height.
+  PoseCommand pose_cmd = mapped.pose;
+  if (!mapped.remote_gyro_enabled) {
+    pose_cmd.roll_deg = 0;
+    pose_cmd.pitch_deg = 0;
+  }
+  pose.setCommand(pose_cmd);
+  pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0,
+                      mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0);
+  pose.update(dt);
+
+  FootTarget gait_targets[NUM_LEGS];
+  bool in_stance[NUM_LEGS];
+
+  if (state == RobotState::WALKING) {
+    gait.update(dt, mapped.twist, gait_targets);
+    for (int i = 0; i < NUM_LEGS; i++) in_stance[i] = gait.legInStance(i);
+  } else {
+    // Sit/stand: blend between a crouched and a neutral stance height, no
+    // stepping. This assumes "sit" keeps all six feet grounded - if your
+    // sit pose rests the chassis on the ground instead, replace this with
+    // a proper sequenced transition (feet positioned first, then lift).
+    float blend = state_machine.sitStandBlend();
+    for (int i = 0; i < NUM_LEGS; i++) {
+      FootTarget neutral = Kinematics::neutralStance(i);
+      FootTarget sit_target = { neutral.x, neutral.y, neutral.z * 0.4f };
+      gait_targets[i].x = sit_target.x + (neutral.x - sit_target.x) * blend;
+      gait_targets[i].y = sit_target.y + (neutral.y - sit_target.y) * blend;
+      gait_targets[i].z = sit_target.z + (neutral.z - sit_target.z) * blend;
+      in_stance[i] = true;
+    }
+  }
+
+  bool stable = Limits::stancePolygonOk(in_stance);
+  (void)stable;   // hook this up to your own response once you're ready - slow down, warn, etc.
+
+  constexpr float MAX_JOINT_DEG_S = 300.0f;   // measure your actual servo speed and set this for real
+
+  for (int i = 0; i < NUM_LEGS; i++) {
+    FootTarget posed = pose.apply(gait_targets[i]);
+    FootTarget clamped = Limits::clampWorkspace(i, posed);
+    JointAngles angles = Kinematics::solve(i, clamped);
+
+    angles.coxa_deg  = Limits::clampJointRate(last_joint_deg[i][0], angles.coxa_deg,  MAX_JOINT_DEG_S, dt);
+    angles.femur_deg = Limits::clampJointRate(last_joint_deg[i][1], angles.femur_deg, MAX_JOINT_DEG_S, dt);
+    angles.tibia_deg = Limits::clampJointRate(last_joint_deg[i][2], angles.tibia_deg, MAX_JOINT_DEG_S, dt);
+    last_joint_deg[i][0] = angles.coxa_deg;
+    last_joint_deg[i][1] = angles.femur_deg;
+    last_joint_deg[i][2] = angles.tibia_deg;
+
+    if (state != RobotState::DISARMED) {
+      servo_driver.writeLeg(i, angles);
+    }
+  }
+}

2026-current/scripts/select_src_dir.py

@@ -0,0 +1,16 @@
+# PlatformIO pre-build script: point each environment at its own sketch folder.
+#
+# PlatformIO only has one global `src_dir`, and it only converts .ino files in
+# the root of that directory. Each env sets `custom_src_dir = robot` (or
+# `remote`) and this script swaps PROJECT_SRC_DIR before the build starts, so
+# robot/ and remote/ stay plain Arduino-style sketch folders (one .ino plus
+# paired .h/.cpp files) that also open in the Arduino IDE.
+import os
+
+Import("env")  # noqa: F821 - injected by PlatformIO/SCons
+
+sub = env.GetProjectOption("custom_src_dir")  # noqa: F821
+src = os.path.join(env.subst("$PROJECT_DIR"), sub)  # noqa: F821
+if not os.path.isdir(src):
+    raise SystemExit("custom_src_dir '%s' does not exist" % src)
+env.Replace(PROJECT_SRC_DIR=src)  # noqa: F821