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/robot.ino

@@ -8,6 +8,7 @@
 #include "Limits.h"
 #include "Comms.h"
 #include "ServoDriver.h"
+#include "SmoothedValue.h"
 
 // Uncomment to drive the robot from the Serial Monitor before a remote exists.
 // See README.md for the key map.
@@ -22,6 +23,8 @@ Comms comms;
 ServoDriver servo_driver;
 
 float current_heading_deg = 0;          // open-loop: accumulates commanded omega. See note in loop().
+SmoothedValue stance_height(STANCE_HEIGHT_RATE_MM_S);   // remote height slider, rate-limited
+float gyro_corr_roll_deg = 0, gyro_corr_pitch_deg = 0;  // robot-gyro self-leveling (integrated)
 float last_joint_deg[NUM_LEGS][3] = {}; // for per-joint rate limiting
 uint32_t last_loop_us = 0;
 RobotState last_state = RobotState::DISARMED;
@@ -41,8 +44,9 @@ void pollSerialTestInput() {
       case 'x': test_packet.vx = max(test_packet.vx - 200, -1000); test_packet.flags |= FLAG_WALK; break;
       case 'q': test_packet.vy = min(test_packet.vy + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
       case 'e': test_packet.vy = max(test_packet.vy - 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;
+      // yaw_rate + = turn left (CCW).
+      case 'z': test_packet.yaw_rate = min(test_packet.yaw_rate + 200, 1000);  test_packet.flags |= FLAG_WALK; break;
+      case 'c': test_packet.yaw_rate = max(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 ' ':
@@ -64,6 +68,7 @@ void setup() {
   gait.begin();
   pose.begin();
   state_machine.begin();
+  stance_height.reset(STANCE_HEIGHT_MM);
 
   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");
@@ -87,15 +92,20 @@ void loop() {
 
   MapperOutput mapped = mapper.map(cmd, current_heading_deg, dt);
 
+  // Standing height: every neutral foot position (gait, sit/stand, limits)
+  // follows this, so it's set before anything below uses them.
+  stance_height.setTarget(mapped.stance_height_mm);
+  Kinematics::setStanceHeight(stance_height.update(dt));
+
+  // Slow the walk command down to a stride the legs can really take (reach,
+  // no neighbouring legs colliding) - the stride alone; the body pose then
+  // makes room for it below.
+  gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
+  mapped.twist = gait.limitTwist(mapped.twist, GaitEngine::NO_POSE);
+
   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();
 
@@ -105,23 +115,55 @@ void loop() {
   } else if (state != RobotState::DISARMED && last_state == RobotState::DISARMED) {
     servo_driver.begin();
   }
+  // The stand pose holds every foot at neutral, so walking starts from there.
+  if (state == RobotState::WALKING && last_state != RobotState::WALKING) gait.resetFeet();
   last_state = state;
 
-  gait.setRequestedPattern(mapped.gait_wave ? &GAIT_WAVE : &GAIT_TRIPOD);
-
   // Stick pose + remote-tilt feedforward (gated on FLAG_REMOTE_GYRO) are
   // already combined in InputMapper.
   pose.setCommand(mapped.pose);
-  pose.setCorrection(mapped.robot_gyro_enabled ? -imu.rollDeg() : 0,
-                      mapped.robot_gyro_enabled ? -imu.pitchDeg() : 0);
+
+  // Robot-gyro self-leveling. The IMU measures the body's tilt in the world,
+  // which already includes the correction applied so far - so the correction
+  // integrates (a plain "correction = -measured tilt" only halves a slope).
+  // The tilt the operator asked for is subtracted first, so leveling doesn't
+  // cancel the stick. Switched off, the correction bleeds away smoothly.
+  if (mapped.robot_gyro_enabled) {
+    gyro_corr_roll_deg  -= ROBOT_GYRO_KI * (imu.rollDeg()  - pose.commandRollDeg())  * dt;
+    gyro_corr_pitch_deg -= ROBOT_GYRO_KI * (imu.pitchDeg() - pose.commandPitchDeg()) * dt;
+    gyro_corr_roll_deg  = constrain(gyro_corr_roll_deg,  -ROBOT_GYRO_MAX_CORR_DEG, ROBOT_GYRO_MAX_CORR_DEG);
+    gyro_corr_pitch_deg = constrain(gyro_corr_pitch_deg, -ROBOT_GYRO_MAX_CORR_DEG, ROBOT_GYRO_MAX_CORR_DEG);
+  } else {
+    float step = POSE_TILT_MAX_RATE_DEG * dt;
+    gyro_corr_roll_deg  = constrain(0.0f, gyro_corr_roll_deg - step,  gyro_corr_roll_deg + step);
+    gyro_corr_pitch_deg = constrain(0.0f, gyro_corr_pitch_deg - step, gyro_corr_pitch_deg + step);
+  }
+  pose.setCorrection(gyro_corr_roll_deg, gyro_corr_pitch_deg);
   pose.update(dt);
 
   FootTarget gait_targets[NUM_LEGS];
   bool in_stance[NUM_LEGS];
 
   if (state == RobotState::WALKING) {
-    gait.update(dt, mapped.twist, gait_targets);
+    // Walking has priority over the body pose: the pose shrinks until the
+    // stride the walk command wants fits (and where the feet are right now,
+    // since a stride in transition can be outside that envelope). The pose
+    // shrinks at a bounded rate - no snapping - so meanwhile the walk slows
+    // down against the pose as it is, and everything stays physical.
+    FootTarget sets[GaitEngine::ENVELOPE_SETS + 1][NUM_LEGS];
+    gait.strideEnvelope(mapped.twist, sets);
+    for (int i = 0; i < NUM_LEGS; i++) sets[GaitEngine::ENVELOPE_SETS][i] = gait.feet()[i];
+    pose.fitToReach(sets, GaitEngine::ENVELOPE_SETS + 1, dt);
+    Twist twist = gait.limitTwist(mapped.twist, pose, true);
+
+    gait.update(dt, twist, pose, gait_targets);
     for (int i = 0; i < NUM_LEGS; i++) in_stance[i] = gait.legInStance(i);
+
+    // Simple open-loop heading estimate: accumulates the omega the planted
+    // feet actually carried out (only while walking - the body only turns
+    // then). 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.
+    current_heading_deg += degrees(gait.appliedTwist().omega) * dt;
   } else {
     // Sit/stand: blend between a crouched and a neutral stance height, no
     // stepping. This assumes "sit" keeps all six feet grounded - if your
@@ -136,6 +178,8 @@ void loop() {
       gait_targets[i].z = sit_target.z + (neutral.z - sit_target.z) * blend;
       in_stance[i] = true;
     }
+    // Shrink the body pose as a whole until it's physically possible here.
+    pose.fitToReach(&gait_targets, 1, dt);
   }
 
   bool stable = Limits::stancePolygonOk(in_stance);