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

@@ -30,17 +30,33 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
     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);
+    // Hold the current heading, so switching to mode B doesn't turn the
+    // robot toward some old target.
+    heading_target_deg_ = heading_deg;
+    stick_active_ = false;
   } else {
-    // Mode B: heading-hold. Stick angle becomes a target heading; forward
-    // speed comes from stick magnitude. yaw_rate is unused here.
+    // Mode B: steering, relative to where the body faces right now. The
+    // stick's forward/back component sets the walking direction, its sideways
+    // component steers: forward walks straight on, back walks straight back,
+    // forward+left or back+left walk while turning left, left alone turns on
+    // the spot (for as long as it's held). Let go and the robot holds the
+    // heading it has at that moment. yaw_rate is unused here.
     float mag = sqrt(raw_vx * raw_vx + raw_vy * raw_vy);
+    float direction = raw_vx < 0 ? -1.0f : 1.0f;
     if (mag > 0.05f) {
-      heading_target_deg_ = degrees(atan2(raw_vy, raw_vx));
+      stick_active_ = true;
+      heading_target_deg_ = wrapDeg(heading_deg + degrees(atan2(raw_vy, fabs(raw_vx))));
+    } else if (stick_active_) {
+      stick_active_ = false;
+      heading_target_deg_ = heading_deg;   // released: hold this heading
     }
     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
+    // Along the robot's current facing (forward or back), scaled by how well
+    // it already faces the target: a hard turn happens mostly on the spot.
+    float facing = max(0.0f, cos(radians(heading_error)));
+    out.twist.vx = direction * mag * facing * MAX_LINEAR_MM_S;
     out.twist.vy = 0;
   }
 
@@ -66,5 +82,7 @@ MapperOutput InputMapper::map(const CmdPacket& p, float heading_deg, float dt) {
                                    -MAX_TILT_DEG, MAX_TILT_DEG);
   }
 
+  out.stance_height_mm = STANCE_HEIGHT_MM + normalize(p.body_height) * STANCE_HEIGHT_RANGE_MM;
+
   return out;
 }