Firmware simulation, Unity project, and sim-verified robot fixes

Simulation
- sim/: the unchanged robot firmware compiled into hexapod_sim.dll
  against PC stand-ins (Arduino, Servo, Wire + register-level MPU6050,
  RF24 with matching-config radio), a kinematic body model (support
  plane + planted-foot odometry) that also drives the simulated IMU,
  and a headless smoke test (sim_smoke). build.ps1 builds and tests.
- Hexapod_Simulation_Unity/ folded in (history kept in a bundle
  outside the repo). Assets/HexapodSim loads the DLL, drives the
  existing leg rig, and offers joystick / keyboard / robot-serial
  input plus a test panel.

Robot (all found or confirmed in the simulation)
- Config.h geometry taken from the Unity model.
- IK femur elevation sign, gait stance direction, body pose
  rotation direction, yaw lever arm.
- Stance feet integrated from the current command; gait restarts at
  mid-stance; heading integrates the applied turn rate, only while
  walking.
- Leg clearance and reach checks (Limits::feetFeasible); walk command
  and body pose scaled as a whole, pose scale rate-limited, planted
  feet guarded so the body never snaps or legs collide.
- Robot-gyro self-leveling as an integrator.
- Heading mode: steering relative to the body; S walks backward.
- Standing height from the remote (STANCE_HEIGHT_RANGE_MM).

Protocol v3 (reflash both boards)
- CmdPacket gains body_height (22 bytes).

Remote
- Height slider (Nextion hHeight/nHeight, serial [ ]).
- Fixed unspecified I2C read order in RemoteIMU.

LEGACY/: owner's old Nextion HMI and remote hardware assets.

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

2026-current/robot/GaitEngine.cpp

@@ -1,4 +1,5 @@
 #include "GaitEngine.h"
+#include "Limits.h"
 
 const GaitPattern GAIT_TRIPOD = {
   0.5f,
@@ -11,16 +12,79 @@ const GaitPattern GAIT_WAVE = {
 };
 
 void GaitEngine::begin() {
+  resetFeet();
+}
+
+// Every foot starts at neutral, which is the *middle* of a stride. So the
+// cycle restarts with the pattern shifted to put leg 0 at mid-stance (for
+// the tripod that puts every grounded leg at mid-stance); a grounded leg
+// that started at the beginning of its stance would instead run a full
+// stride past neutral into the workspace clamp and stop pushing. Legs that
+// land in their swing phase lift off from neutral over the remaining swing.
+void GaitEngine::resetFeet() {
   for (int i = 0; i < 6; i++) {
     legs_pattern_[i] = requested_pattern_;
-    leg_phase_[i] = legs_pattern_[i]->leg_phase_offset[i];
+    const GaitPattern* p = legs_pattern_[i];
+    float phase = p->leg_phase_offset[i] + p->duty_factor * 0.5f;
+    leg_phase_[i] = phase - floorf(phase);
+    foot_[i] = Kinematics::neutralStance(i);
+    liftoff_[i] = foot_[i];
+    swing_start_[i] = leg_phase_[i] < p->duty_factor ? p->duty_factor : leg_phase_[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]) {
+void GaitEngine::strideEnvelope(const Twist& twist, FootTarget sets[ENVELOPE_SETS][6]) const {
+  float half = cycle_sec_ * requested_pattern_->duty_factor * 0.5f;
   for (int i = 0; i < 6; i++) {
+    FootTarget n = Kinematics::neutralStance(i);
+    float dx = (twist.vx - twist.omega * n.y) * half;   // half-stride: body velocity at the foot
+    float dy = (twist.vy + twist.omega * n.x) * half;
+    float sign = (i % 2 == 0) ? 1.0f : -1.0f;
+    sets[0][i] = { n.x + sign * dx, n.y + sign * dy, n.z };
+    sets[1][i] = { n.x - sign * dx, n.y - sign * dy, n.z };
+    sets[2][i] = { n.x, n.y, n.z + SWING_LIFT_MM };
+  }
+}
+
+const BodyPose GaitEngine::NO_POSE{};
+
+bool GaitEngine::envelopeFeasible(const Twist& twist, const BodyPose& pose) const {
+  FootTarget sets[ENVELOPE_SETS][6];
+  strideEnvelope(twist, sets);
+  for (int k = 0; k < ENVELOPE_SETS; k++)
+    if (!pose.feasible(sets[k])) return false;
+  return true;
+}
+
+// Neighbouring legs step in opposite phases, so each one's stride eats into
+// the gap between them from both sides; turning on the spot at full rate
+// asks for ~60 mm half-strides at the outer feet, enough to make the legs
+// meet. Rather than letting single feet clamp (or collide), the whole
+// command is slowed until the stride fits.
+Twist GaitEngine::limitTwist(const Twist& twist, const BodyPose& pose, bool chained) {
+  float s = 1.0f;
+  if (!envelopeFeasible(twist, pose)) {
+    float lo = 0.0f, hi = 1.0f;
+    for (int n = 0; n < 8; n++) {
+      float mid = 0.5f * (lo + hi);
+      Twist t = { twist.vx * mid, twist.vy * mid, twist.omega * mid };
+      if (envelopeFeasible(t, pose)) lo = mid; else hi = mid;
+    }
+    s = lo;
+  }
+  twist_scale_ = chained ? twist_scale_ * s : s;
+  return { twist.vx * s, twist.vy * s, twist.omega * s };
+}
+
+void GaitEngine::update(float dt, const Twist& twist, const BodyPose& pose, FootTarget out[6]) {
+  // Pass 1: advance phases, place swing feet, and work out how far each
+  // planted foot would move this tick.
+  bool stance[6];
+  FootTarget step[6] = {};
+  for (int i = 0; i < 6; i++) {
+    bool was_stance = leg_phase_[i] < legs_pattern_[i]->duty_factor;
     leg_phase_[i] += dt / cycle_sec_;
 
     // Touchdown: this leg's cycle wraps back to 0 - the one moment it's safe
@@ -33,33 +97,68 @@ void GaitEngine::update(float dt, const Twist& twist, FootTarget out[6]) {
 
     const GaitPattern* p = legs_pattern_[i];
     bool in_stance = leg_phase_[i] < p->duty_factor;
+    stance[i] = in_stance;
 
     FootTarget neutral = Kinematics::neutralStance(i);
     float stride_time = cycle_sec_ * p->duty_factor;
+    FootTarget& foot = foot_[i];
 
-    // 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;
+      // Planted foot: stays put in the world, so in the body frame it moves
+      // opposite to the body's velocity at that point (v + omega x r).
+      // Integrated every tick, so the body moves exactly as commanded even
+      // when the command changes mid-stride.
+      step[i].x = -(twist.vx - twist.omega * foot.y) * dt;
+      step[i].y = -(twist.vy + twist.omega * foot.x) * dt;
       foot.z = neutral.z;
     } else {
-      float t = (leg_phase_[i] - p->duty_factor) / (1.0f - p->duty_factor);
+      if (was_stance) {                      // just lifted off
+        liftoff_[i] = foot;
+        swing_start_[i] = p->duty_factor;
+      }
+
+      // Swing toward the leading end of the next stride: the point that a
+      // full stance at the current command carries back through neutral.
+      float half = stride_time * 0.5f;
+      FootTarget touchdown = {
+        neutral.x + (twist.vx - twist.omega * neutral.y) * half,
+        neutral.y + (twist.vy + twist.omega * neutral.x) * half,
+        neutral.z
+      };
+      float t = (leg_phase_[i] - swing_start_[i]) / (1.0f - swing_start_[i]);
       float st = smoothstep(t);
-      foot.x = end.x + (start.x - end.x) * st;
-      foot.y = end.y + (start.y - end.y) * st;
+      foot.x = liftoff_[i].x + (touchdown.x - liftoff_[i].x) * st;
+      foot.y = liftoff_[i].y + (touchdown.y - liftoff_[i].y) * st;
       foot.z = neutral.z + SWING_LIFT_MM * sin(PI * t);
     }
-    out[i] = foot;
+  }
+
+  // Pass 2: the planted feet move together - they're what carries the body.
+  // limitTwist() sizes the command for a steady stride, but when the command
+  // changes mid-step (a key pressed while a foot has just landed) a planted
+  // foot can be asked to travel further than its leg reaches, or into its
+  // neighbour. Then all planted feet move less, by one common factor: the
+  // body slows down rather than one foot being clamped out of formation.
+  float s = 1.0f;
+  FootTarget proposed[6];
+  auto propose = [&](float k) {
+    for (int i = 0; i < 6; i++)
+      proposed[i] = stance[i] ? FootTarget{ foot_[i].x + step[i].x * k, foot_[i].y + step[i].y * k, foot_[i].z }
+                              : foot_[i];
+    return pose.feasible(proposed);
+  };
+  if (!propose(1.0f)) {
+    float lo = 0.0f, hi = 1.0f;
+    for (int n = 0; n < 6; n++) {
+      float mid = 0.5f * (lo + hi);
+      if (propose(mid)) lo = mid; else hi = mid;
+    }
+    s = lo;
+    propose(s);
+  }
+  applied_twist_ = { twist.vx * s, twist.vy * s, twist.omega * s };
+  for (int i = 0; i < 6; i++) {
+    foot_[i] = proposed[i];
+    out[i] = foot_[i];
   }
 }