Restore closed-loop sim telemetry + add CSV-driven test runner

.gitignore

@@ -0,0 +1,11 @@
+# In-repo Arduino library wrapper at Non_grapic_simulator/libraries/ is set up
+# so Arduino IDE finds our shared simulator_physics library. Any OTHER
+# libraries a developer installs via Tools -> Manage Libraries (e.g. due_can)
+# will also land there because that's now the sketchbook libraries dir.
+# Ignore everything in libraries/ EXCEPT our own simulator_physics so
+# third-party libs don't pollute the repo.
+Non_grapic_simulator/libraries/*
+!Non_grapic_simulator/libraries/simulator_physics/
+
+# Router sim output
+Non_grapic_simulator/**/SIM*.CSV

NavigateTestRunner/NavigateTestRunner.ino

@@ -0,0 +1,218 @@
+/*
+ * NavigateTestRunner
+ * Sensor Hub Arduino Due — PC-driven test gateway for the closed-loop sim.
+ *
+ * Sister sketch to ../Navigate/Navigate.ino. Where Navigate runs the full
+ * autonomous waypoint mission, this sketch turns Sensor Hub into a thin
+ * gateway between the PC's test runner (tools/test_runner.py) and the CAN
+ * bus, so PC-side test CSVs can drive the through-DBW closed loop and
+ * score the result.
+ *
+ *   Inbound  (PC -> SH):  CMD,<id>,<speed>,<brake>,<mode>,<angle>\n
+ *   Outbound (SH -> PC):  ACK,<millis>\n
+ *                         LOG,<millis>,<key=val>,...\n
+ *
+ * Flow per loop (~100 ms):
+ *   1. Drain CAN RX -> latch current pose + actual angle + router angle
+ *   2. Parse any pending CMD,... line from SerialUSB
+ *   3. Once a CMD has been latched, publish 0x350 NavDrive + 0x100
+ *      NavStatus every loop and emit a LOG line for the scorer
+ *   4. Spin-drain CAN until the loop period elapses (keeps the RX
+ *      mailboxes fresh even while DBW floods 0x701..0x70A logger frames)
+ *
+ * To switch between autonomous nav and test gateway, flash the other
+ * sketch — there is no in-firmware mode toggle.
+ *
+ * See tools/test_runner.py for the PC-side runner that drives this and
+ * scores the resulting LOG stream.
+ */
+
+#include <due_can.h>
+
+// ===== CAN IDs =====
+// Per https://www.elcanoproject.org/wiki/Communication
+// SH -> DBW : 0x350 NavDrive (speed/brake/angle), 0x100 NavStatus (auto/estop)
+// DBW -> SH : 0x400 Actual (actual speed + actual wheel angle)
+// Router -> SH : 0x4C0 position, 0x4E0 heading, 0x4F0 speed
+// Router -> DBW (snooped by SH for second-witness logging): 0x430 SimSteerActual
+#define NavDrive_CANID          0x350
+#define NavStatus_CANID         0x100
+#define Actual_CANID            0x400
+#define VehiclePosition_CANID   0x4C0
+#define VehicleHeading_CANID    0x4E0
+#define VehicleSpeed_CANID      0x4F0
+#define SimSteerActual_CANID    0x430
+
+// ===== Loop period =====
+#define LOOP_PERIOD_MS 100
+
+// ===== Pose state captured from CAN =====
+static int32_t currentEast_cm           = 0;
+static int32_t currentNorth_cm          = 0;
+static int16_t currentHeading_centiDeg  = 0;   // 0 = north, 9000 = east
+static int16_t currentSpeed_cmPs        = 0;
+static int16_t actualSteerAngle_DegX10  = 0;   // from DBW 0x400 bytes 4-5
+static int16_t routerAngle_DegX10       = 0;   // from Router 0x430 — second witness
+
+// ===== Latched command from PC =====
+// Republished on CAN every loop once the first CMD arrives. brake defaults
+// to 100 (engaged) so DBW holds the trike still if a test forgets to send
+// brake=0 before commanding speed > 0.
+static int16_t  cmd_speed_cmPs   = 0;
+static int16_t  cmd_brake        = 100;
+static uint8_t  cmd_mode         = 0;
+static int16_t  cmd_angle_DegX10 = 0;
+static bool     cmdLatched       = false;
+static char     cmdBuf[64];
+static int      cmdBufIdx        = 0;
+
+// ===== Forward declarations =====
+static void drainCanRx();
+static void readTestCommand();
+static void publishCmdFrames();
+static void printTestLog();
+
+/* -------------------------------------------------------------------------- */
+void setup() {
+  SerialUSB.begin(115200);
+  // Native USB enumerates after sketch boot. Bounded wait so the sketch
+  // boots even when no host is attached.
+  uint32_t waitStart = millis();
+  while (!SerialUSB && (millis() - waitStart) < 3000);
+
+  // CAN at 500 kbps (matches DBW + Router). Catch-all RX filter so all
+  // frames hit our drain loop.
+  Can0.begin(CAN_BPS_500K);
+  Can0.watchFor();
+
+  // Boot banner the PC test runner watches for to confirm it's talking
+  // to the test gateway and not autonomous Navigate.
+  SerialUSB.println("RDY,test_runner_v0");
+}
+
+/* -------------------------------------------------------------------------- */
+void loop() {
+  uint32_t loopStart = millis();
+
+  drainCanRx();
+  readTestCommand();
+
+  if (cmdLatched) {
+    publishCmdFrames();
+    printTestLog();
+  }
+
+  // Spin-drain CAN for the rest of the loop period so RX mailboxes stay
+  // emptied while DBW's logger frames flood the bus.
+  while ((millis() - loopStart) < LOOP_PERIOD_MS) {
+    drainCanRx();
+  }
+}
+
+/* -------------------------------------------------------------------------- */
+/* Capture pose + actual-angle frames from CAN                                */
+/* -------------------------------------------------------------------------- */
+static void drainCanRx() {
+  CAN_FRAME incoming;
+  while (Can0.available() > 0) {
+    Can0.read(incoming);
+    switch (incoming.id) {
+      case VehiclePosition_CANID:
+        if (incoming.length >= 8) {
+          currentEast_cm  = incoming.data.int32[0];
+          currentNorth_cm = incoming.data.int32[1];
+        }
+        break;
+      case VehicleHeading_CANID:
+        if (incoming.length >= 2) {
+          currentHeading_centiDeg = incoming.data.int16[0];
+        }
+        break;
+      case VehicleSpeed_CANID:
+        if (incoming.length >= 2) {
+          currentSpeed_cmPs = incoming.data.int16[0];
+        }
+        break;
+      case Actual_CANID:
+        if (incoming.length >= 6) {
+          // bytes 4-5: actual steer angle (deg x 10)
+          actualSteerAngle_DegX10 = incoming.data.int16[2];
+        }
+        break;
+      case SimSteerActual_CANID:
+        if (incoming.length >= 2) {
+          // Router's view of the simulated wheel angle. Snooped here so the
+          // scorer can compare DBW-reported actual_angle vs Router-reported
+          // router_angle and detect disagreement.
+          routerAngle_DegX10 = incoming.data.int16[0];
+        }
+        break;
+      default:
+        // Ignore everything else (RC, logger frames, etc.)
+        break;
+    }
+  }
+}
+
+/* -------------------------------------------------------------------------- */
+/* Parse CMD,<id>,<speed>,<brake>,<mode>,<angle> lines from PC                */
+/* -------------------------------------------------------------------------- */
+static void readTestCommand() {
+  while (SerialUSB.available() > 0) {
+    char c = (char)SerialUSB.read();
+    if (c == '\n' || c == '\r') {
+      cmdBuf[cmdBufIdx] = '\0';
+      int id, speed, brake, mode, angle;
+      if (cmdBufIdx > 4 &&
+          sscanf(cmdBuf, "CMD,%i,%i,%i,%i,%i",
+                 &id, &speed, &brake, &mode, &angle) == 5) {
+        cmd_speed_cmPs   = (int16_t)speed;
+        cmd_brake        = (int16_t)brake;
+        cmd_mode         = (uint8_t)mode;
+        cmd_angle_DegX10 = (int16_t)angle;
+        cmdLatched       = true;
+        SerialUSB.print("ACK,"); SerialUSB.println(millis());
+      }
+      cmdBufIdx = 0;
+    } else if (cmdBufIdx < (int)sizeof(cmdBuf) - 1) {
+      cmdBuf[cmdBufIdx++] = c;
+    }
+  }
+}
+
+/* -------------------------------------------------------------------------- */
+/* Publish the latched CMD as 0x350 NavDrive + 0x100 NavStatus every loop     */
+/* -------------------------------------------------------------------------- */
+static void publishCmdFrames() {
+  CAN_FRAME f;
+  f.extended = false;
+
+  // 0x350 NavDrive: int16 speed_cmPs, int16 brake, int16 angle_DegX10
+  f.id = NavDrive_CANID;
+  f.length = 6;
+  f.data.int16[0] = cmd_speed_cmPs;
+  f.data.int16[1] = cmd_brake;
+  f.data.int16[2] = cmd_angle_DegX10;
+  Can0.sendFrame(f);
+
+  // 0x100 NavStatus: uint8 status. Bit 6 (0x40) asserts autonomous mode
+  // so DBW arbitrates to AUTO_RC and actually follows the 0x350 commands.
+  f.id = NavStatus_CANID;
+  f.length = 1;
+  f.data.uint8[0] = 0x40;
+  Can0.sendFrame(f);
+}
+
+/* -------------------------------------------------------------------------- */
+/* Emit one machine-parseable LOG line for the PC-side scorer                 */
+/* -------------------------------------------------------------------------- */
+static void printTestLog() {
+  SerialUSB.print("LOG,");                  SerialUSB.print(millis());
+  SerialUSB.print(",east_cm=");             SerialUSB.print(currentEast_cm);
+  SerialUSB.print(",north_cm=");            SerialUSB.print(currentNorth_cm);
+  SerialUSB.print(",heading_centiDeg=");    SerialUSB.print(currentHeading_centiDeg);
+  SerialUSB.print(",actual_angle_tenths="); SerialUSB.print(actualSteerAngle_DegX10);
+  SerialUSB.print(",router_angle_tenths="); SerialUSB.print(routerAngle_DegX10);
+  SerialUSB.print(",cmd_speed_cmPs=");      SerialUSB.print(cmd_speed_cmPs);
+  SerialUSB.print(",cmd_angle_tenths=");    SerialUSB.println(cmd_angle_DegX10);
+}

Non_grapic_simulator/README.md

@@ -0,0 +1,95 @@
+# Non_grapic_simulator — Router Arduino Due sketches
+
+This folder contains the Router-side simulator sketches for the Elcano trike
+project plus a shared physics library.
+
+```
+Non_grapic_simulator/
+├── README.md                              ← you are here
+├── libraries/
+│   └── simulator_physics/                 ← shared physics, single source of truth
+│       ├── library.properties
+│       └── src/simulator_physics.h
+├── simulator_stage1/                      ← standalone Router sketch (Minhee)
+│   └── simulator_stage1.ino
+└── simulator_closed_loop/                 ← bridge closed-loop test sketch (Toprak)
+    └── simulator_closed_loop.ino
+```
+
+## Required one-time setup (per developer)
+
+Both sketches `#include <simulator_physics.h>` from the in-repo Arduino
+library at `libraries/simulator_physics/`. For the Arduino IDE to find that
+library, you must point its **Sketchbook Location** at this folder.
+
+**Without this step the sketches will not compile** — the IDE will report
+`fatal error: simulator_physics.h: No such file or directory`.
+
+### Arduino IDE 2.x
+
+1. **File → Preferences** (on macOS: **Arduino IDE → Settings**)
+2. Set the **Sketchbook location** field to the full path of *this folder*
+   (i.e. wherever your local clone of the repo puts `Non_grapic_simulator/`).
+   Examples:
+   ```
+   Windows : C:\path\to\Bridge\Non_grapic_simulator
+   macOS   : /Users/<you>/path/to/Bridge/Non_grapic_simulator
+   Linux   : /home/<you>/path/to/Bridge/Non_grapic_simulator
+   ```
+3. Click **OK**
+4. **Quit Arduino IDE completely**, then reopen it. (Some versions need the
+   restart for library indexing to pick up the new path.)
+
+### Arduino IDE 1.x
+
+Same idea — **File → Preferences → Sketchbook location**.
+
+## Install third-party libraries (closed-loop sketch only)
+
+`simulator_closed_loop.ino` uses the **due_can** library to broadcast CAN
+frames. `simulator_stage1.ino` does not need it.
+
+Side effect of changing the Sketchbook Location: the Arduino IDE no longer
+sees libraries that were installed in your *old* sketchbook (e.g. the default
+`Documents/Arduino/libraries/due_can/`). Reinstall the dependencies into the
+new sketchbook:
+
+1. With Sketchbook Location set to this folder, open Arduino IDE
+2. **Tools → Manage Libraries…**
+3. Search for `due_can` and install (by Collin Kidder)
+
+The library will land at `libraries/due_can/`. It's `.gitignore`d so it
+won't get committed.
+
+## Verify the setup worked
+
+Before clicking Verify on either sketch, open **Sketch → Include Library**.
+You should see `simulator_physics` listed under "Contributed libraries" or
+"Custom libraries". If it's there, includes will resolve.
+
+If it isn't there:
+
+- The Sketchbook Location path is probably wrong. Open Preferences and confirm
+  it points to **this folder** (the one containing both `libraries/` and the
+  two sketch folders), not to `libraries/` itself and not to a sketch folder.
+- Try restarting the IDE again.
+
+## Why this folder layout?
+
+Arduino IDE only searches a few places for `#include "..."` and
+`#include <...>`: the active sketch folder, installed library `src/` folders,
+the IDE-bundled libraries, and the Arduino core. It does **not** traverse up
+into parent directories. Putting `simulator_physics.h` directly at the
+project root would not work — sketches in subfolders wouldn't find it.
+
+The library wrapper (`libraries/simulator_physics/`) is how Arduino IDE
+exposes a single shared file to multiple sketches. The wrapper folders are
+the Arduino convention, not decoration.
+
+## Switching back to your normal sketchbook
+
+If you do Elcano work in the morning and other Arduino projects in the
+evening, you'll need to flip Sketchbook Location back to your default
+(usually `Documents/Arduino`) for those projects. Arduino IDE 2.x supports
+multiple windows — you can keep one window pointed here for Elcano and
+another pointed at your default sketchbook for everything else.

Non_grapic_simulator/libraries/simulator_physics/library.properties

@@ -0,0 +1,10 @@
+name=simulator_physics
+version=1.0.0
+author=Elcano project
+maintainer=Elcano project
+sentence=Shared physics engine for Elcano Router simulator sketches.
+paragraph=Provides integer sin/cos, throttle/brake/momentum speed model, steering integration, position dead reckoning, wheel-tick output, and L/R sensor DAC output. Used by simulator_stage1 (standalone) and simulator_closed_loop (Bridge closed-loop test).
+category=Other
+url=https://github.com/elcano/Bridge
+architectures=sam
+includes=simulator_physics.h