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/BodyPose.h

@@ -20,12 +20,35 @@ public:
 
   void update(float dt);
 
-  // Applies the current (smoothed) pose transform to a body-frame foot target.
+  // Scales the whole pose down (one factor for all six legs) until every set
+  // of six feet in `sets` is still physically possible with the pose applied
+  // (reachable, no colliding legs). Pass the stride envelope while walking,
+  // so the pose fits the whole stride rather than this tick's feet, or the
+  // stand targets otherwise. Call after update() and before apply().
+  void fitToReach(const FootTarget sets[][6], int count, float dt);
+  float reachScale() const { return reach_scale_; }   // 1 = full commanded pose
+
+  // The operator's tilt as actually applied (smoothed and reach-scaled),
+  // without the gyro correction - what self-leveling should keep.
+  float commandRollDeg() const  { return s_roll_.current * reach_scale_; }
+  float commandPitchDeg() const { return s_pitch_.current * reach_scale_; }
+
+  // Applies the current (smoothed, reach-scaled) pose transform to a body-frame foot target.
   FootTarget apply(const FootTarget& in) const;
 
+  // True if these (un-posed) feet are physically possible with the current
+  // pose applied - see Limits::feetFeasible.
+  bool feasible(const FootTarget feet[6]) const;
+
 private:
   PoseCommand command_;
   float correction_roll_ = 0, correction_pitch_ = 0;
+  float reach_scale_ = 1.0f;
+  static constexpr float REACH_RECOVER_PER_SEC = 1.0f;
+  static constexpr float REACH_DROP_PER_SEC = 3.0f;
+
+  FootTarget applyScaled(const FootTarget& in, float scale) const;
+  bool reachable(const FootTarget sets[][6], int count, float scale) const;
 
   SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
   SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};