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/sim/SimHardware.cpp

@@ -0,0 +1,222 @@
+// Implementations of the Arduino/Teensy stand-ins declared in shim/.
+#include "SimHardware.h"
+#include <Arduino.h>
+#include <Servo.h>
+#include <Wire.h>
+#include <RF24.h>
+#include <EEPROM.h>
+#include <HexapodProtocol.h>
+#include <stdarg.h>
+#include <deque>
+#include <string>
+#include <vector>
+
+SimSerial Serial;
+TwoWire Wire;
+SimEEPROM EEPROM;
+
+// =====================================================================
+// Clock
+// =====================================================================
+static uint64_t g_now_us = 0;
+
+namespace simhw {
+void advanceMicros(uint32_t us) { g_now_us += us; }
+uint64_t nowMicros() { return g_now_us; }
+}  // namespace simhw
+
+uint32_t micros() { return (uint32_t)g_now_us; }
+uint32_t millis() { return (uint32_t)(g_now_us / 1000); }
+// The robot loop never blocks; if it ever does, time just moves on.
+void delay(uint32_t ms) { g_now_us += (uint64_t)ms * 1000; }
+void delayMicroseconds(uint32_t us) { g_now_us += us; }
+
+// =====================================================================
+// Serial
+// =====================================================================
+static std::deque<char> g_serial_in;
+static std::string g_serial_out;
+
+namespace simhw {
+void serialFeed(const char* bytes, int len) {
+  for (int i = 0; i < len; i++) g_serial_in.push_back(bytes[i]);
+}
+int serialDrain(char* out, int cap) {
+  int n = (int)g_serial_out.size() < cap ? (int)g_serial_out.size() : cap;
+  memcpy(out, g_serial_out.data(), n);
+  g_serial_out.erase(0, n);
+  return n;
+}
+}  // namespace simhw
+
+int SimSerial::available() { return (int)g_serial_in.size(); }
+int SimSerial::read() {
+  if (g_serial_in.empty()) return -1;
+  char c = g_serial_in.front();
+  g_serial_in.pop_front();
+  return (uint8_t)c;
+}
+size_t SimSerial::write(uint8_t c) {
+  if (g_serial_out.size() < 65536) g_serial_out.push_back((char)c);   // cap if nobody drains
+  return 1;
+}
+size_t SimSerial::print(const char* s) {
+  size_t n = 0;
+  while (*s) n += write((uint8_t)*s++);
+  return n;
+}
+size_t SimSerial::printf(const char* fmt, ...) {
+  char buf[256];
+  va_list ap;
+  va_start(ap, fmt);
+  vsnprintf(buf, sizeof(buf), fmt, ap);
+  va_end(ap);
+  return print(buf);
+}
+
+// =====================================================================
+// Servos
+// =====================================================================
+static simhw::ServoPin g_pins[simhw::MAX_PINS];
+
+namespace simhw {
+const ServoPin& servoPin(int pin) {
+  static const ServoPin none;
+  return (pin >= 0 && pin < MAX_PINS) ? g_pins[pin] : none;
+}
+}  // namespace simhw
+
+uint8_t Servo::attach(int pin) {
+  if (pin < 0 || pin >= simhw::MAX_PINS) return 0;
+  pin_ = pin;
+  if (!g_pins[pin].attached) g_pins[pin].us = 1500;
+  g_pins[pin].attached = true;
+  return 1;
+}
+void Servo::detach() {
+  if (pin_ >= 0) g_pins[pin_].attached = false;
+  pin_ = -1;
+}
+void Servo::writeMicroseconds(int us) {
+  if (pin_ < 0) return;
+  g_pins[pin_].us = us;
+  g_pins[pin_].writes++;
+}
+
+// =====================================================================
+// I2C + MPU6050 (register-level, address 0x68)
+// =====================================================================
+static constexpr int MPU_ADDR = 0x68;
+static bool g_imu_present = true;
+static uint8_t g_mpu_regs[128];
+static uint8_t g_mpu_ptr = 0;
+static int g_tx_addr = -1;
+static std::vector<uint8_t> g_tx_bytes;
+static std::deque<uint8_t> g_rx;
+
+namespace simhw {
+void imuSetPresent(bool present) { g_imu_present = present; }
+
+static void putBE(uint8_t reg, float value, float scale) {
+  float raw = value * scale;
+  if (raw > 32767) raw = 32767;
+  if (raw < -32768) raw = -32768;
+  int16_t v = (int16_t)lroundf(raw);
+  g_mpu_regs[reg]     = (uint8_t)((uint16_t)v >> 8);
+  g_mpu_regs[reg + 1] = (uint8_t)(v & 0xFF);
+}
+
+void imuSet(float ax, float ay, float az, float gx, float gy, float gz) {
+  // Default +/-2 g and +/-250 deg/s ranges, same scale factors RobotIMU uses.
+  putBE(0x3B, ax, 16384.0f);
+  putBE(0x3D, ay, 16384.0f);
+  putBE(0x3F, az, 16384.0f);
+  putBE(0x41, 0, 1);   // temperature, unused
+  putBE(0x43, gx, 131.0f);
+  putBE(0x45, gy, 131.0f);
+  putBE(0x47, gz, 131.0f);
+}
+}  // namespace simhw
+
+void TwoWire::beginTransmission(int address) {
+  g_tx_addr = address;
+  g_tx_bytes.clear();
+}
+size_t TwoWire::write(uint8_t b) {
+  g_tx_bytes.push_back(b);
+  return 1;
+}
+uint8_t TwoWire::endTransmission(bool) {
+  if (g_tx_addr != MPU_ADDR || !g_imu_present) return 2;   // address NACK
+  if (!g_tx_bytes.empty()) {
+    g_mpu_ptr = g_tx_bytes[0] & 0x7F;                       // first byte = register pointer
+    for (size_t i = 1; i < g_tx_bytes.size(); i++) g_mpu_regs[(g_mpu_ptr++) & 0x7F] = g_tx_bytes[i];
+    if (g_tx_bytes.size() > 1) g_mpu_ptr = g_tx_bytes[0] & 0x7F;
+  }
+  return 0;
+}
+uint8_t TwoWire::requestFrom(int address, int quantity) {
+  g_rx.clear();
+  if (address != MPU_ADDR || !g_imu_present) return 0;
+  for (int i = 0; i < quantity; i++) g_rx.push_back(g_mpu_regs[(g_mpu_ptr + i) & 0x7F]);
+  return (uint8_t)quantity;
+}
+int TwoWire::available() { return (int)g_rx.size(); }
+int TwoWire::read() {
+  if (g_rx.empty()) return -1;
+  uint8_t b = g_rx.front();
+  g_rx.pop_front();
+  return b;
+}
+
+// =====================================================================
+// nRF24L01 "air"
+// =====================================================================
+struct AirPacket {
+  uint8_t channel;
+  rf24_datarate_e rate;
+  uint8_t address[5];
+  std::vector<uint8_t> payload;
+};
+static bool g_radio_present = true;
+static std::deque<AirPacket> g_air;   // packets waiting at the receiver
+static constexpr size_t RX_FIFO_DEPTH = 3;
+
+namespace simhw {
+void radioSetPresent(bool present) { g_radio_present = present; }
+void radioSend(const uint8_t* bytes, int len) {
+  if (len < 0 || len > 32) return;
+  if (g_air.size() >= RX_FIFO_DEPTH) return;   // FIFO full: the real chip drops it too
+  AirPacket p;
+  p.channel = HXP_RADIO_CHANNEL;
+  p.rate = (rf24_datarate_e)HXP_RADIO_DATA_RATE;
+  memcpy(p.address, HXP_RADIO_ADDRESS, 5);
+  p.payload.assign(bytes, bytes + len);
+  g_air.push_back(p);
+}
+}  // namespace simhw
+
+bool RF24::begin() { return g_radio_present; }
+bool RF24::isChipConnected() { return g_radio_present; }
+void RF24::openReadingPipe(uint8_t, const uint8_t* address) {
+  memcpy(address_, address, 5);
+  pipe_open_ = true;
+}
+bool RF24::available() {
+  if (!g_radio_present || !listening_ || !pipe_open_) return false;
+  // Drop anything this radio's configuration wouldn't have decoded.
+  while (!g_air.empty()) {
+    const AirPacket& p = g_air.front();
+    bool match = p.channel == channel_ && p.rate == data_rate_ &&
+                 memcmp(p.address, address_, 5) == 0 && p.payload.size() == payload_size_;
+    if (match) return true;
+    g_air.pop_front();
+  }
+  return false;
+}
+void RF24::read(void* buf, uint8_t len) {
+  if (g_air.empty()) return;
+  const AirPacket& p = g_air.front();
+  memcpy(buf, p.payload.data(), len < p.payload.size() ? len : p.payload.size());
+  g_air.pop_front();
+}