From 30bcffdde2437777eec1a855bc48f9c74ac0a022 Mon Sep 17 00:00:00 2001 From: toprakcelikel Date: Sun, 14 Jun 2026 03:37:00 -0700 Subject: [PATCH 1/3] Add simulator_closed_loop sketch and shared physics library Restores the CAN backbone for end-to-end closed-loop testing that PR #14 inadvertently dropped, as a sister sketch alongside simulator_stage1 (refactored only to share the physics engine; runtime behavior unchanged). - New simulator_closed_loop sketch: reads real DBW inputs (throttle, L_TURN, R_TURN), publishes 0x4C0 position, 0x4E0 heading, 0x4F0 speed, and 0x430 SimSteerActual via CAN for Sensor Hub Nav and DBW to close their respective loops. - Shared simulator_physics Arduino library used by both sketches. - LOG_PORT macro with USE_NATIVE_USB switch (default on for Bridge board) so the Native USB port is used and D0/D1 stay quiet for the inter-board UART. - analogWriteResolution(12), PIO_PUDR pull-up disable on L_TURN/R_TURN, MAX_SPEED reduced to 2000 mm/s for waypoint capture turn radius. - README documents the one-time Arduino IDE Sketchbook Location change and the library install copy from the default sketchbook. - .gitignore keeps third-party libraries (due_can, Time, RTClib, etc.) out of the repo while still tracking our own simulator_physics. End-to-end closed loop verified on bench against DBW main (PR #7). --- .gitignore | 11 + Non_grapic_simulator/README.md | 95 +++ .../simulator_physics/library.properties | 10 + .../simulator_physics/src/simulator_physics.h | 158 +++++ .../simulator_closed_loop.ino | 584 ++++++++++++++++++ .../simulator_stage1/simulator_stage1.ino | 119 +--- 6 files changed, 880 insertions(+), 97 deletions(-) create mode 100644 .gitignore create mode 100644 Non_grapic_simulator/README.md create mode 100644 Non_grapic_simulator/libraries/simulator_physics/library.properties create mode 100644 Non_grapic_simulator/libraries/simulator_physics/src/simulator_physics.h create mode 100644 Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a57415b --- /dev/null +++ b/.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 diff --git a/Non_grapic_simulator/README.md b/Non_grapic_simulator/README.md new file mode 100644 index 0000000..d2bd85e --- /dev/null +++ b/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 ` 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//path/to/Bridge/Non_grapic_simulator + Linux : /home//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. diff --git a/Non_grapic_simulator/libraries/simulator_physics/library.properties b/Non_grapic_simulator/libraries/simulator_physics/library.properties new file mode 100644 index 0000000..c39df50 --- /dev/null +++ b/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 diff --git a/Non_grapic_simulator/libraries/simulator_physics/src/simulator_physics.h b/Non_grapic_simulator/libraries/simulator_physics/src/simulator_physics.h new file mode 100644 index 0000000..1ddef5e --- /dev/null +++ b/Non_grapic_simulator/libraries/simulator_physics/src/simulator_physics.h @@ -0,0 +1,158 @@ +#pragma once + +/* + * Shared physics engine for the Router simulator sketches. + * + * Used by: + * simulator_stage1/simulator_stage1.ino (standalone variant) + * simulator_closed_loop/simulator_closed_loop.ino (bridge closed-loop variant) + * + * Each sketch includes this header with: + * #include + * + * --- IDE setup (one-time per developer) --- + * This is an Arduino library bundled in the repo at + * Non_grapic_simulator/libraries/simulator_physics/. For the Arduino IDE to + * find it, set: + * File -> Preferences -> Sketchbook location: /Non_grapic_simulator + * Then both sketches resolve via the IDE's standard + * library search path. Alternatively, symlink (or copy) the library folder + * into ~/Arduino/libraries/. + * + * --- Implementation notes --- + * Functions are `static inline` so each sketch gets its own translation-unit + * copy without linker conflicts. They reference macros and globals that the + * including sketch MUST define BEFORE the include line. Required: + * + * Macros: + * ANGLE_CHANGE_TENTHS, MAX_ANGLE_TENTHS + * MIN_EFFECTIVE_THROTTLE, MAX_EFFECTIVE_THROTTLE, MAX_SPEED_mmPs + * FRICTION_NUM, FRICTION_DEN + * THROTTLE_HISTORY, THROTTLE_DELAY_START, THROTTLE_DELAY_END + * WHEEL_CIRCUM_MM, LOOP_TIME_MS + * L_STRAIGHT, L_MIN, L_MAX, R_STRAIGHT, R_MIN, R_MAX + * IRPT_WHEEL_PIN, L_SENSE_PIN, R_SENSE_PIN + * + * Globals declared in the .ino: + * int throttleHistory[THROTTLE_HISTORY]; + * int historyIndex; + * int prevSpeed_mmPs; + * int angle_tenths; + * long X_mm, Y_mm; + * unsigned long nextPulseTime_ms; + * + * If the two sketches diverge on a constant (e.g. MAX_SPEED_mmPs: + * standalone=13600, closed-loop=2000), each gets the correct value + * because macros resolve at each .ino's compilation, not here. + * + * Brake model in computeSpeed: half-decay per loop while braking + * (introduced in PR #14 by Minhee; preserved here). + */ + +// =========================================================================== +// Integer sine/cosine (returns value * 1000) +// =========================================================================== +static inline int sin1000(int angle_tenths) { + angle_tenths = ((angle_tenths % 3600) + 3600) % 3600; + static const int sinTable[91] = { + 0, 17, 35, 52, 70, 87, 105, 122, 139, 156, + 174, 191, 208, 225, 242, 259, 276, 292, 309, 326, + 342, 358, 375, 391, 407, 423, 438, 454, 469, 485, + 500, 515, 530, 545, 559, 574, 588, 602, 616, 629, + 643, 656, 669, 682, 695, 707, 719, 731, 743, 755, + 766, 777, 788, 799, 809, 819, 829, 839, 848, 857, + 866, 875, 883, 891, 899, 906, 914, 921, 927, 934, + 940, 946, 951, 956, 961, 966, 970, 974, 978, 982, + 985, 988, 990, 993, 995, 996, 998, 999, 999, 1000, + 1000 + }; + int deg = angle_tenths / 10; + if (angle_tenths < 900) return sinTable[deg]; + else if (angle_tenths < 1800) return sinTable[180 - deg]; + else if (angle_tenths < 2700) return -sinTable[deg - 180]; + else return -sinTable[360 - deg]; +} + +static inline int cos1000(int angle_tenths) { + return sin1000(angle_tenths + 900); +} + +// =========================================================================== +// Compute Speed (integer arithmetic). +// Brake model: decay speed by half each loop while braking to model +// brake-deceleration ramp, instead of instantly zeroing. +// =========================================================================== +static inline int computeSpeed(int throttle, bool brakeOn) { + if (brakeOn) { prevSpeed_mmPs = prevSpeed_mmPs * 5000 / 10000; return prevSpeed_mmPs; } + long sum = 0; + for (int i = THROTTLE_DELAY_START; i <= THROTTLE_DELAY_END; i++) { + int idx = (historyIndex - i + THROTTLE_HISTORY) % THROTTLE_HISTORY; + sum += throttleHistory[idx]; + } + int meanThrottle = (int)(sum / 8); + int estimatedSpeed = 0; + if (meanThrottle > MIN_EFFECTIVE_THROTTLE) + estimatedSpeed = (int)((long)MAX_SPEED_mmPs * + (meanThrottle - MIN_EFFECTIVE_THROTTLE) / + (MAX_EFFECTIVE_THROTTLE - MIN_EFFECTIVE_THROTTLE)); + if (estimatedSpeed < 0) estimatedSpeed = 0; + int momentum = (int)((long)prevSpeed_mmPs * FRICTION_NUM / FRICTION_DEN); + int newSpeed = (momentum > estimatedSpeed) ? momentum : estimatedSpeed; + if (newSpeed > MAX_SPEED_mmPs) newSpeed = MAX_SPEED_mmPs; + prevSpeed_mmPs = newSpeed; + return newSpeed; +} + +// =========================================================================== +// Update Steering Angle (integrates L_TURN/R_TURN inputs each loop) +// =========================================================================== +static inline int updateAngle(bool lTurn, bool rTurn) { + if (lTurn && !rTurn) angle_tenths -= ANGLE_CHANGE_TENTHS; + else if (!lTurn && rTurn) angle_tenths += ANGLE_CHANGE_TENTHS; + if (angle_tenths > MAX_ANGLE_TENTHS) angle_tenths = MAX_ANGLE_TENTHS; + if (angle_tenths < -MAX_ANGLE_TENTHS) angle_tenths = -MAX_ANGLE_TENTHS; + return angle_tenths; +} + +// =========================================================================== +// Update Vehicle Position (dead-reckon X/Y from speed/heading/angle) +// =========================================================================== +static inline void updatePosition(int speed, int &heading, int angle) { + if (speed > 0) heading += angle / 10; + if (heading >= 3600) heading -= 3600; + if (heading < 0) heading += 3600; + int distance_mm = speed * LOOP_TIME_MS / 1000; + X_mm += (long)distance_mm * sin1000(heading) / 1000; + Y_mm += (long)distance_mm * cos1000(heading) / 1000; +} + +// =========================================================================== +// Send wheel-tick pulse to DBW at a rate proportional to speed +// =========================================================================== +static inline void sendWheelPulse(int speed_mmPs) { + if (speed_mmPs <= 0) return; + unsigned long pulseInterval_ms = (unsigned long)WHEEL_CIRCUM_MM * 1000 / speed_mmPs; + unsigned long now = millis(); + if (now >= nextPulseTime_ms) { + digitalWrite(IRPT_WHEEL_PIN, HIGH); + delayMicroseconds(100); + digitalWrite(IRPT_WHEEL_PIN, LOW); + nextPulseTime_ms = now + pulseInterval_ms; + } +} + +// =========================================================================== +// Drive L_SENSE / R_SENSE DACs to simulate wheel-angle pot voltages +// =========================================================================== +static inline void sendAngleSensor(int angleT) { + int lSense, rSense; + if (angleT >= 0) { + lSense = L_STRAIGHT + (L_MIN - L_STRAIGHT) * angleT / MAX_ANGLE_TENTHS; + rSense = R_STRAIGHT + (R_MIN - R_STRAIGHT) * angleT / MAX_ANGLE_TENTHS; + } else { + lSense = L_STRAIGHT + (L_MAX - L_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; + rSense = R_STRAIGHT + (R_MAX - R_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; + } + analogWrite(L_SENSE_PIN, lSense * 4); + analogWrite(R_SENSE_PIN, rSense * 4); +} diff --git a/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino b/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino new file mode 100644 index 0000000..12bcb9d --- /dev/null +++ b/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino @@ -0,0 +1,584 @@ +/* + * Simulator — Closed-Loop Integration variant + * Router Arduino Due + * + * SISTER SKETCH to ../simulator_stage1/simulator_stage1.ino. + * simulator_stage1 : standalone / standalone-Due use, no CAN, simple + * test mode with fixed throttle. Owned by Minhee. + * simulator_closed_loop: end-to-end closed-loop test on the Bridge board. + * Reads real DBW inputs (throttle / L_TURN / R_TURN), + * publishes pose + actual wheel angle on CAN so the + * Sensor Hub Nav + DBW close their loops through it. + * + * The two sketches share the same integer physics engine (sin1000, cos1000, + * computeSpeed, updateAngle, updatePosition, sendWheelPulse, + * sendAngleSensor). Those functions are mirrored here verbatim. If the + * physics changes in simulator_stage1.ino, mirror it here too (or factor + * out to a shared header later). + * + * --- Hardware I/O --- + * Inputs from DBW Arduino (via Bridge harness): + * A0 -> THROTTLE (from DBW DAC0) + * D42 -> BRAKE_VOLT (from DBW D40) + * D48 -> BRAKE_ON (from DBW D44) + * D4 -> L_TURN (from DBW D26): HIGH while DBW wants to turn left + * D2 -> R_TURN (from DBW D28): HIGH while DBW wants to turn right + * + * Outputs to DBW Arduino: + * D47 -> IRPT_WHEEL (wheel-tick pulse, rate scales with speed) + * DAC0 -> L_SENSE (left wheel angle pot voltage) + * DAC1 -> R_SENSE (right wheel angle pot voltage) + * + * CAN frames Router transmits (Router does not RX any frames): + * 0x4C0 position (east_cm int32 + north_cm int32, 8 bytes) + * 0x4E0 heading (heading_centiDeg int16, 2 bytes) + * 0x4F0 speed (speed_cmPs int16, 2 bytes) + * 0x430 steer-actual (angle_DegX10 int16, 2 bytes) — substitutes for the + * L_SENSE/R_SENSE analog feedback that the bridge + * harness can't carry reliably. DBW uses this as the + * measured wheel angle for its PID. On the real trike + * DBW would read its own analog sensors instead. + * + * --- Stage 2 ASCII command pipe (PC -> Router via USB) --- + * Format: "CANID,nbytes,data1,data2,...\n" + * Example: "350,6,1500,0,1,-21" -> CAN ID 0x350, speed=1500 cm/s, brake=0, + * mode=1, angle=-21 (=-2.1 deg left). Lets a PC script drive Router + * without DBW being present. + * + * Logs CSV to SD card if available, falls back to USB Serial (LOG_PORT). + * CSV: time_ms, X_mm, Y_mm, heading_tenths, speed_mmPs, angle_tenths, throttle, brakeOn + * + * All arithmetic uses integers per professor's requirement. + * Angles stored as tenths of a degree. Positions in mm. Speed in mm/s. + * + * Reference: https://www.elcanoproject.org/wiki/Communication + */ + +// ============================================================================ +// SETUP REQUIRED — this sketch will not compile until the Arduino IDE's +// Sketchbook Location is pointed at the parent folder of this sketch: +// File -> Preferences -> Sketchbook location: /Non_grapic_simulator +// The IDE then finds the shared `simulator_physics` library at +// ../libraries/simulator_physics/ +// See ../README.md for full setup details + troubleshooting. +// ============================================================================ + +#include +#include +#include + +// ===== Logging / USB Serial port ===== +// LOG_PORT is used for ALL host-side I/O: CSV log, debug prints, Stage 2 +// ASCII command input, Stage 3 binary 0x251/0x4C0 packets. +// +// USE_NATIVE_USB defined (default for this sketch) -> SerialUSB (Native USB port) +// USE_NATIVE_USB commented out -> Serial (Programming port) +// +// This sketch defaults to SerialUSB because it's intended to be flashed onto +// a Router Due sitting on the Bridge board. The Bridge harness wires +// Router's D0/D1 (the Serial UART pins) to DBW's Serial1, so Serial.print() +// would also send bytes onto that inter-board wire and garbage DBW's debug +// stream. SerialUSB routes through the Native USB jack and leaves D0/D1 +// quiet. +// +// If you ever want to run this sketch on a standalone Due (no Bridge), comment +// out the line below and plug into the Programming USB port instead. +#define USE_NATIVE_USB + +#ifdef USE_NATIVE_USB + #define LOG_PORT SerialUSB +#else + #define LOG_PORT Serial +#endif + +// ===== Pin Definitions ===== +// From DBW DAC0 +#define THROTTLE_PIN A0 +// From DBW D40 +#define BRAKE_VOLT_PIN 42 +// From DBW D44 +#define BRAKE_ON_PIN 48 +// From DBW D26 — HIGH while DBW wants to turn left +#define L_TURN_PIN 4 +// From DBW D28 — HIGH while DBW wants to turn right +#define R_TURN_PIN 2 +// To DBW D47 +#define IRPT_WHEEL_PIN 47 +// To DBW A10 +#define L_SENSE_PIN DAC0 +// To DBW A11 +#define R_SENSE_PIN DAC1 + +// ===== SD Card Pin ===== +#define SD_CS_PIN 37 + +// ===== CAN ID Definitions ===== +// Per https://www.elcanoproject.org/wiki/Communication +// Nav->DBW: speed(cm/s), brake, mode, angle(deg x10) +#define CAN_DRIVE 0x350 +// Router->Sensor Hub: vehicle position east_cm, north_cm +#define CAN_POSITION 0x4C0 +// Router->Sensor Hub: heading_centiDeg (compass only) +#define CAN_HEADING 0x4E0 +// Router->Sensor Hub: speed_cmPs +#define CAN_SPEED 0x4F0 +// Router->DBW: simulated actual wheel angle (deg x10). +// Substitutes for L_SENSE/R_SENSE analog feedback that is unavailable on +// this bridge. On the real trike, DBW would read its analog sensors. +#define CAN_STEER_ACTUAL 0x430 +// Nav->all: GPS origin lat/lon +#define CAN_ORIGIN 0x251 + +// ===== Origin GPS coordinates (UW Bothell) ===== +// latitude degrees +#define ORIGIN_LAT 47 +// latitude fraction (0-9,999,999) +#define ORIGIN_LAT_FRAC 760934 +// longitude degrees +#define ORIGIN_LON 122 +// longitude fraction (0-999,999), West +#define ORIGIN_LON_FRAC 189963 + +// ===== Speed Model Constants ===== +#define FRICTION_NUM 9296 +#define FRICTION_DEN 10000 +#define MIN_EFFECTIVE_THROTTLE 65 +#define MAX_EFFECTIVE_THROTTLE 227 +// 2 m/s cap — slow enough that turn radius (~3 m) fits within the waypoint +// capture radius. simulator_stage1 uses 13600 (13.6 m/s); that's too fast +// for the closed-loop Nav waypoint logic to track without overshooting. +#define MAX_SPEED_mmPs 2000 +#define THROTTLE_HISTORY 10 +#define THROTTLE_DELAY_START 3 +#define THROTTLE_DELAY_END 10 + +// ===== Vehicle Settings ===== +#define WHEEL_DIAMETER_MM 495 +#define WHEEL_CIRCUM_MM 1555 +#define LOOP_TIME_MS 100 +#define MAX_ANGLE_TENTHS 250 +// ===== Wheel Angle Sensor ===== +#define ANGLE_CHANGE_TENTHS 20 +#define L_STRAIGHT 722 +#define L_MIN 779 +#define L_MAX 639 +#define R_STRAIGHT 731 +#define R_MIN 673 +#define R_MAX 786 +// ===== Global Variables (all integer) ===== +int throttleHistory[THROTTLE_HISTORY]; +int historyIndex = 0; + +int speed_mmPs = 0; +int prevSpeed_mmPs = 0; +int heading_tenths = 0; +int angle_tenths = 0; + +long X_mm = 0; +long Y_mm = 0; + +unsigned long nextPulseTime_ms = 0; + +// ===== Stage 2: ASCII command state ===== +bool useScriptCommand = false; +// commanded speed from PC (cm/s) +int cmd_speed_cmPs = 0; +// 0=off, 1=hold(12V), 2=on(24V) +int cmd_brake = 0; +// 0=init,1=RC,2=operator,3=auto-RC,4=auto-op +int cmd_mode = 0; +// commanded steer angle (degrees x10) +int cmd_angle_tenths = 0; + +// ===== SD Card Variables ===== +File logFile; +bool sdAvailable = false; + +// ===== Function Declarations ===== +// sin1000, cos1000, computeSpeed, updateAngle, updatePosition, +// sendWheelPulse, sendAngleSensor are provided by ../simulator_physics.h. +void readScriptCommand(); +void sendPosition(); +void sendOrigin(); +bool sendPositionCAN(); +bool sendHeadingCAN(); +bool sendSpeedCAN(); +bool sendSteerActualCAN(); + +// Shared physics engine — provided by the in-repo Arduino library at +// Non_grapic_simulator/libraries/simulator_physics. Provides: sin1000, +// cos1000, computeSpeed, updateAngle, updatePosition, sendWheelPulse, +// sendAngleSensor. See the header for macro/global preconditions. +// Included AFTER the global variable declarations above because the inline +// function bodies reference those globals (prevSpeed_mmPs, angle_tenths, +// X_mm, Y_mm, historyIndex, throttleHistory[], nextPulseTime_ms) by name. +// SETUP: in Arduino IDE, set File -> Preferences -> Sketchbook location to +// /Non_grapic_simulator so the IDE finds the library. +#include + +// =========================================================================== +// Setup +// =========================================================================== +void setup() { + LOG_PORT.begin(115200); +#ifdef USE_NATIVE_USB + // Native USB enumerates after sketch boot. Bounded wait so the sketch + // boots even when no host is attached. + uint32_t waitStart = millis(); + while (!LOG_PORT && (millis() - waitStart) < 3000); +#endif + + // Configure pins + pinMode(THROTTLE_PIN, INPUT); + pinMode(BRAKE_VOLT_PIN, INPUT); + pinMode(BRAKE_ON_PIN, INPUT); + pinMode(L_TURN_PIN, INPUT); + pinMode(R_TURN_PIN, INPUT); + // Explicitly disable the SAM3X internal pullups on the L_TURN/R_TURN + // inputs. Otherwise the boot pullup state is non-deterministic and + // digitalRead can return HIGH even when DBW is actively pulling LOW. + g_APinDescription[L_TURN_PIN].pPort->PIO_PUDR = g_APinDescription[L_TURN_PIN].ulPin; + g_APinDescription[R_TURN_PIN].pPort->PIO_PUDR = g_APinDescription[R_TURN_PIN].ulPin; + pinMode(IRPT_WHEEL_PIN, OUTPUT); + digitalWrite(IRPT_WHEEL_PIN, LOW); + + // Due DAC is 12-bit (0-4095). Default analogWrite resolution is 8-bit, + // which truncates the lSense*4 / rSense*4 values in sendAngleSensor and + // saturates DBW's A10/A11 readings. Set 12-bit so calibration matches. + analogWriteResolution(12); + + for (int i = 0; i < THROTTLE_HISTORY; i++) throttleHistory[i] = 0; + + // Initialize CAN at 500 kbps (matches DBW + Sensor Hub). + // Router only transmits — no RX mailboxes needed. + if (Can0.begin(CAN_BPS_500K)) { + LOG_PORT.println("CAN init OK"); + } else { + LOG_PORT.println("CAN init FAILED"); + } + + // Stage 3 binary USB origin (0x251) intentionally not sent at boot — + // Sensor Hub Nav reads pose over CAN (0x4C0/0x4E0/0x4F0). The + // sendOrigin() function is kept defined for backwards compat with the + // older PC simulator pipe. + + // Initialize SD card + LOG_PORT.print("Initializing SD card on pin D"); + LOG_PORT.print(SD_CS_PIN); + LOG_PORT.print("... "); + pinMode(SD_CS_PIN, OUTPUT); + digitalWrite(SD_CS_PIN, HIGH); + delay(100); + if (SD.begin(SD_CS_PIN)) { + LOG_PORT.println("SD card found!"); + char filename[13]; + for (int i = 0; i < 100; i++) { + sprintf(filename, "SIM%02d.CSV", i); + if (!SD.exists(filename)) break; + } + logFile = SD.open(filename, FILE_WRITE); + if (logFile) { + sdAvailable = true; + LOG_PORT.print("Logging to SD: "); + LOG_PORT.println(filename); + } else { + LOG_PORT.println("Failed to open file. Using USB Serial."); + } + } else { + LOG_PORT.println("SD not found. Using USB Serial."); + } + + if (sdAvailable) { + logFile.println("time_ms,X_mm,Y_mm,heading_tenths,speed_mmPs,angle_tenths,throttle,brakeOn"); + logFile.flush(); + } else { + LOG_PORT.println("time_ms,X_mm,Y_mm,heading_tenths,speed_mmPs,angle_tenths,throttle,brakeOn"); + } + LOG_PORT.println("Simulator closed-loop variant started."); +} + +// =========================================================================== +// Main Loop +// =========================================================================== +void loop() { + // Runs continuously — no auto-halt; Sensor Hub keeps receiving frames for + // the whole drive. + uint32_t startTime = millis(); + + // Stage 2: read ASCII command from PC if available + readScriptCommand(); + + int throttle; + bool brakeOn; + bool lTurn; + bool rTurn; + + if (useScriptCommand) { + // Drive command received from PC simulator (Stage 2). + // speed and angle are set directly; skip throttle model. + // cm/s -> mm/s + speed_mmPs = cmd_speed_cmPs * 10; + angle_tenths = cmd_angle_tenths; + brakeOn = (cmd_brake > 0); + throttle = 0; + lTurn = false; + rTurn = false; + } else { + // Real inputs from DBW (closes the loop: Nav -> CAN -> DBW -> pins -> here). + // THROTTLE_PIN (A0) <- DBW DAC0 throttle output, 0-1023 analog + // BRAKE_ON_PIN (D48) <- DBW BRAKE_ON_PIN (D44) + // L_TURN_PIN (D4) <- DBW LEFT_TURN_PIN (D26): HIGH = turn left + // R_TURN_PIN (D2) <- DBW RIGHT_TURN_PIN (D28): HIGH = turn right + int rawThrottle = analogRead(THROTTLE_PIN); + throttle = rawThrottle / 4; + // BRAKE derived from throttle below the effective threshold. When Nav + // commands speed=0, DBW's throttle PID outputs 0 and we treat that as brake. + brakeOn = (throttle < MIN_EFFECTIVE_THROTTLE); + + // Steering via two digital wires from DBW (L_TURN D4, R_TURN D2). + lTurn = (digitalRead(L_TURN_PIN) == HIGH); + rTurn = (digitalRead(R_TURN_PIN) == HIGH); + angle_tenths = updateAngle(lTurn, rTurn); + + static int dbgCount = 0; + if (++dbgCount >= 10) { + dbgCount = 0; + LOG_PORT.print("# L="); LOG_PORT.print(lTurn ? 1 : 0); + LOG_PORT.print(" R="); LOG_PORT.print(rTurn ? 1 : 0); + LOG_PORT.print(" ang_tenths="); LOG_PORT.print(angle_tenths); + LOG_PORT.print(" speed="); LOG_PORT.println(speed_mmPs); + } + + throttleHistory[historyIndex] = throttle; + historyIndex = (historyIndex + 1) % THROTTLE_HISTORY; + speed_mmPs = computeSpeed(throttle, brakeOn); + } + + // Update vehicle position + updatePosition(speed_mmPs, heading_tenths, angle_tenths); + + // Simulated sensor outputs to DBW (wheel-tick + L_SENSE/R_SENSE DACs) + sendWheelPulse(speed_mmPs); + sendAngleSensor(angle_tenths); + + // Broadcast position, heading, speed, steer-actual on CAN — one frame each. + static uint32_t txOk = 0, txFail = 0; + if (sendPositionCAN()) txOk++; else txFail++; + if (sendHeadingCAN()) txOk++; else txFail++; + if (sendSpeedCAN()) txOk++; else txFail++; + if (sendSteerActualCAN()) txOk++; else txFail++; + static uint32_t lastTxDbg_ms = 0; + if (millis() - lastTxDbg_ms > 2000) { + lastTxDbg_ms = millis(); + LOG_PORT.print("# Router TX txOk="); LOG_PORT.print(txOk); + LOG_PORT.print(" txFail="); LOG_PORT.println(txFail); + } + + // CSV log row + if (sdAvailable) { + logFile.print(millis()); logFile.print(","); + logFile.print(X_mm); logFile.print(","); + logFile.print(Y_mm); logFile.print(","); + logFile.print(heading_tenths); logFile.print(","); + logFile.print(speed_mmPs); logFile.print(","); + logFile.print(angle_tenths); logFile.print(","); + logFile.print(throttle); logFile.print(","); + logFile.println(brakeOn ? 1 : 0); + logFile.flush(); + } else { + LOG_PORT.print(millis()); LOG_PORT.print(","); + LOG_PORT.print(X_mm); LOG_PORT.print(","); + LOG_PORT.print(Y_mm); LOG_PORT.print(","); + LOG_PORT.print(heading_tenths); LOG_PORT.print(","); + LOG_PORT.print(speed_mmPs); LOG_PORT.print(","); + LOG_PORT.print(angle_tenths); LOG_PORT.print(","); + LOG_PORT.print(throttle); LOG_PORT.print(","); + LOG_PORT.println(brakeOn ? 1 : 0); + } + + uint32_t elapsed = millis() - startTime; + if (elapsed < LOOP_TIME_MS) delay(LOOP_TIME_MS - elapsed); +} + +// =========================================================================== +// Stage 2: ASCII drive command from PC via USB Serial +// =========================================================================== +// Format: "CANID,nbytes,data1,data2,...\n" +// Example: "350,6,1500,0,1,-21" +// CAN ID 0x350, 6 data bytes +// speed=1500 cm/s, brake=0, mode=1, angle=-21 (=-2.1 deg left) +// Optional execution time prefix (ms): "1000,350,6,1500,0,1,-21" +// Lines starting with '#' are comments and ignored. +void readScriptCommand() { + static char buf[64]; + static int bufIdx = 0; + + while (LOG_PORT.available()) { + char c = (char)LOG_PORT.read(); + if (c == '\n' || c == '\r') { + buf[bufIdx] = '\0'; + bufIdx = 0; + + // Skip empty lines and comments + if (buf[0] == '\0' || buf[0] == '#') return; + + // Parse comma-separated integer fields + int fields[12]; + int nFields = 0; + char *ptr = buf; + while (*ptr && nFields < 12) { + fields[nFields++] = (int)strtol(ptr, &ptr, 0); + if (*ptr == ',') ptr++; + } + if (nFields < 3) return; + + // If first value > 0x7FF it is an execution time prefix; skip it + int idx = (fields[0] > 0x7FF) ? 1 : 0; + if (nFields < idx + 3) return; + + int canId = fields[idx]; + // fields[idx+1] = nbytes (not needed directly) + // speed (cm/s) + int d0 = (nFields > idx+2) ? fields[idx+2] : 0; + // brake + int d1 = (nFields > idx+3) ? fields[idx+3] : 0; + // mode + int d2 = (nFields > idx+4) ? fields[idx+4] : 0; + // angle (deg x10) + int d3 = (nFields > idx+5) ? fields[idx+5] : 0; + + if (canId == CAN_DRIVE) { + // 0x350: speed(cm/s), brake, mode, steer angle(deg x10) + cmd_speed_cmPs = d0; + cmd_brake = d1; + cmd_mode = d2; + cmd_angle_tenths = d3; + useScriptCommand = true; + // Debug echo so PC can verify Router received the command + LOG_PORT.print("CMD: speed="); LOG_PORT.print(cmd_speed_cmPs); + LOG_PORT.print(" brake="); LOG_PORT.print(cmd_brake); + LOG_PORT.print(" mode="); LOG_PORT.print(cmd_mode); + LOG_PORT.print(" angle="); LOG_PORT.println(cmd_angle_tenths); + } + // Future: handle other CAN IDs as needed + } else { + if (bufIdx < 63) buf[bufIdx++] = c; + } + } +} + +// =========================================================================== +// CAN frame senders (Router -> bus, called once per loop) +// =========================================================================== + +// Note on `f.data` access: the due_can BytesUnion has members +// uint64_t value; +// uint32_t low, high; +// uint16_t s0, s1, s2, s3; +// uint8_t bytes[8]; +// (no int32/int16 arrays). The Arduino Due is little-endian, so writing the +// uint16/uint32 fields here lays the bytes out LE on the wire as the wiki +// protocol expects. We cast the signed input through the matching unsigned +// type so the bit pattern is preserved without sign-extension surprises. + +// 0x4C0 — 8 bytes: east_cm (int32 LE) + north_cm (int32 LE) +bool sendPositionCAN() { + CAN_FRAME f; + f.id = CAN_POSITION; + f.extended = false; + f.length = 8; + f.data.low = (uint32_t)(int32_t)(X_mm / 10); // east_cm + f.data.high = (uint32_t)(int32_t)(Y_mm / 10); // north_cm + return Can0.sendFrame(f); +} + +// 0x4E0 — 2 bytes: heading_centiDeg (int16 LE). Internal storage is tenths-of-deg. +bool sendHeadingCAN() { + CAN_FRAME f; + f.id = CAN_HEADING; + f.extended = false; + f.length = 2; + f.data.s0 = (uint16_t)(int16_t)((long)heading_tenths * 10); + return Can0.sendFrame(f); +} + +// 0x4F0 — 2 bytes: speed_cmPs (int16 LE). +bool sendSpeedCAN() { + CAN_FRAME f; + f.id = CAN_SPEED; + f.extended = false; + f.length = 2; + f.data.s0 = (uint16_t)(int16_t)(speed_mmPs / 10); + return Can0.sendFrame(f); +} + +// 0x430 — 2 bytes: angle_DegX10 (int16 LE). Substitutes for the L_SENSE/R_SENSE +// analog wires that the bridge harness doesn't carry. DBW reads this every +// loop and uses it as the measured wheel angle for its steering PID. On a +// real trike DBW would read its analog sensors and ignore this frame. +bool sendSteerActualCAN() { + CAN_FRAME f; + f.id = CAN_STEER_ACTUAL; + f.extended = false; + f.length = 2; + f.data.s0 = (uint16_t)(int16_t)angle_tenths; + return Can0.sendFrame(f); +} + +// =========================================================================== +// Stage 3: Binary USB packets (kept for the older PC simulator pipe) +// =========================================================================== + +// 0x4C0 binary packet to PC via USB. Used by older PC sim that doesn't speak +// CAN; Sensor Hub Nav reads from sendPositionCAN above instead. +// Per wiki: Bytes 1-4 = east_cm (int32), Bytes 5-8 = north_cm (int32) +// Packet header: 2 bytes [ID_HIGH, ID_LOW|(len<<1)] +// 0x4C0 >> 3 = 0x98 -> ID_HIGH +// ((0x4C0 & 0x07) << 5) | (8 << 1) = 0x00 | 0x10 = 0x10 -> ID_LOW|len +void sendPosition() { + int32_t east_cm = (int32_t)(X_mm / 10); + int32_t north_cm = (int32_t)(Y_mm / 10); + + // CAN ID high byte + LOG_PORT.write(0x98); + // CAN ID low bits | data length + LOG_PORT.write(0x10); + // east_cm big-endian int32 + LOG_PORT.write((uint8_t)(east_cm >> 24)); + LOG_PORT.write((uint8_t)(east_cm >> 16)); + LOG_PORT.write((uint8_t)(east_cm >> 8)); + LOG_PORT.write((uint8_t)(east_cm )); + // north_cm big-endian int32 + LOG_PORT.write((uint8_t)(north_cm >> 24)); + LOG_PORT.write((uint8_t)(north_cm >> 16)); + LOG_PORT.write((uint8_t)(north_cm >> 8)); + LOG_PORT.write((uint8_t)(north_cm )); +} + +// 0x251 GPS origin binary packet. Kept for backwards compat; not called at +// boot in this sketch. +// Per wiki Set Origin (0x251): +// Byte 1: lat degrees (7 bits) + N/S (bit 8, 0=N) +// Bytes 2,3,4: lat fraction (0-9,999,999) +// Byte 5: lon degrees +// Byte 6 bit 1: E/W (0=E, 1=W); rest of byte 6 + bytes 7,8: lon fraction +void sendOrigin() { + // Packet header for 0x251, 8 data bytes + // 0x251 >> 3 = 0x4A -> ID_HIGH + // ((0x251 & 0x07) << 5) | (8 << 1) = 0x20 | 0x10 = 0x30 -> ID_LOW|len + LOG_PORT.write(0x4A); + LOG_PORT.write(0x30); + // Byte 1: lat degrees, N hemisphere -> bit 8 = 0 + LOG_PORT.write((uint8_t)ORIGIN_LAT); + // Bytes 2,3,4: lat fraction = 760934 + LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC >> 16)); + LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC >> 8)); + LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC )); + // Byte 5: lon degrees = 122 + LOG_PORT.write((uint8_t)ORIGIN_LON); + // Bytes 6,7,8: W -> first bit of byte 6 = 1, fraction = 189963 + uint32_t lon_field = (1UL << 23) | (uint32_t)ORIGIN_LON_FRAC; + LOG_PORT.write((uint8_t)(lon_field >> 16)); + LOG_PORT.write((uint8_t)(lon_field >> 8)); + LOG_PORT.write((uint8_t)(lon_field )); +} diff --git a/Non_grapic_simulator/simulator_stage1/simulator_stage1.ino b/Non_grapic_simulator/simulator_stage1/simulator_stage1.ino index 211b018..c29b58a 100644 --- a/Non_grapic_simulator/simulator_stage1/simulator_stage1.ino +++ b/Non_grapic_simulator/simulator_stage1/simulator_stage1.ino @@ -39,6 +39,15 @@ * Reference: https://www.elcanoproject.org/wiki/Communication */ +// ============================================================================ +// SETUP REQUIRED — this sketch will not compile until the Arduino IDE's +// Sketchbook Location is pointed at the parent folder of this sketch: +// File -> Preferences -> Sketchbook location: /Non_grapic_simulator +// The IDE then finds the shared `simulator_physics` library at +// ../libraries/simulator_physics/ +// See ../README.md for full setup details + troubleshooting. +// ============================================================================ + #include #include @@ -135,40 +144,20 @@ File logFile; bool sdAvailable = false; // ===== Function Declarations ===== -int computeSpeed(int throttle, bool brakeOn); -int updateAngle(bool lTurn, bool rTurn); -void updatePosition(int speed, int &heading, int angle); -void sendWheelPulse(int speed); -void sendAngleSensor(int angleT); void readScriptCommand(); void sendPosition(); void sendOrigin(); -// ===== Integer sine/cosine (returns value * 1000) ===== -int sin1000(int angle_tenths) { - angle_tenths = ((angle_tenths % 3600) + 3600) % 3600; - static const int sinTable[91] = { - 0, 17, 35, 52, 70, 87, 105, 122, 139, 156, - 174, 191, 208, 225, 242, 259, 276, 292, 309, 326, - 342, 358, 375, 391, 407, 423, 438, 454, 469, 485, - 500, 515, 530, 545, 559, 574, 588, 602, 616, 629, - 643, 656, 669, 682, 695, 707, 719, 731, 743, 755, - 766, 777, 788, 799, 809, 819, 829, 839, 848, 857, - 866, 875, 883, 891, 899, 906, 914, 921, 927, 934, - 940, 946, 951, 956, 961, 966, 970, 974, 978, 982, - 985, 988, 990, 993, 995, 996, 998, 999, 999, 1000, - 1000 - }; - int deg = angle_tenths / 10; - if (angle_tenths < 900) return sinTable[deg]; - else if (angle_tenths < 1800) return sinTable[180 - deg]; - else if (angle_tenths < 2700) return -sinTable[deg - 180]; - else return -sinTable[360 - deg]; -} - -int cos1000(int angle_tenths) { - return sin1000(angle_tenths + 900); -} +// Shared physics engine — provided by the in-repo Arduino library at +// Non_grapic_simulator/libraries/simulator_physics. Provides: sin1000, +// cos1000, computeSpeed, updateAngle, updatePosition, sendWheelPulse, +// sendAngleSensor. See the header for macro/global preconditions. +// Included AFTER the global variable declarations above because the inline +// function bodies reference those globals (prevSpeed_mmPs, angle_tenths, +// X_mm, Y_mm, historyIndex, throttleHistory[], nextPulseTime_ms) by name. +// SETUP: in Arduino IDE, set File -> Preferences -> Sketchbook location to +// /Non_grapic_simulator so the IDE finds the library. +#include // ===== Setup ===== void setup() { @@ -265,6 +254,7 @@ void loop() { // int rawThrottle = analogRead(THROTTLE_PIN); // throttle = rawThrottle / 4; // brakeOn = digitalRead(BRAKE_ON_PIN); + // lTurn = digitalRead(L_TURN_PIN); // rTurn = digitalRead(R_TURN_PIN); @@ -428,70 +418,5 @@ void sendOrigin() { Serial.write((uint8_t)(lon_field )); } -// ===== Compute Speed (integer arithmetic) ===== -int computeSpeed(int throttle, bool brakeOn) { - if (brakeOn) { prevSpeed_mmPs = prevSpeed_mmPs * 5000 / 10000; return prevSpeed_mmPs; } - long sum = 0; - for (int i = THROTTLE_DELAY_START; i <= THROTTLE_DELAY_END; i++) { - int idx = (historyIndex - i + THROTTLE_HISTORY) % THROTTLE_HISTORY; - sum += throttleHistory[idx]; - } - int meanThrottle = (int)(sum / 8); - int estimatedSpeed = 0; - if (meanThrottle > MIN_EFFECTIVE_THROTTLE) - estimatedSpeed = (int)((long)MAX_SPEED_mmPs * - (meanThrottle - MIN_EFFECTIVE_THROTTLE) / - (MAX_EFFECTIVE_THROTTLE - MIN_EFFECTIVE_THROTTLE)); - if (estimatedSpeed < 0) estimatedSpeed = 0; - int momentum = (int)((long)prevSpeed_mmPs * FRICTION_NUM / FRICTION_DEN); - int newSpeed = (momentum > estimatedSpeed) ? momentum : estimatedSpeed; - if (newSpeed > MAX_SPEED_mmPs) newSpeed = MAX_SPEED_mmPs; - prevSpeed_mmPs = newSpeed; - return newSpeed; -} - -// ===== Update Steering Angle ===== -int updateAngle(bool lTurn, bool rTurn) { - if (lTurn && !rTurn) angle_tenths -= ANGLE_CHANGE_TENTHS; - else if (!lTurn && rTurn) angle_tenths += ANGLE_CHANGE_TENTHS; - if (angle_tenths > MAX_ANGLE_TENTHS) angle_tenths = MAX_ANGLE_TENTHS; - if (angle_tenths < -MAX_ANGLE_TENTHS) angle_tenths = -MAX_ANGLE_TENTHS; - return angle_tenths; -} - -// ===== Update Vehicle Position ===== -void updatePosition(int speed, int &heading, int angle) { - if (speed > 0) heading += angle / 10; - if (heading >= 3600) heading -= 3600; - if (heading < 0) heading += 3600; - int distance_mm = speed * LOOP_TIME_MS / 1000; - X_mm += (long)distance_mm * sin1000(heading) / 1000; - Y_mm += (long)distance_mm * cos1000(heading) / 1000; -} - -// ===== Send Wheel Pulse to DBW ===== -void sendWheelPulse(int speed_mmPs) { - if (speed_mmPs <= 0) return; - unsigned long pulseInterval_ms = (unsigned long)WHEEL_CIRCUM_MM * 1000 / speed_mmPs; - unsigned long now = millis(); - if (now >= nextPulseTime_ms) { - digitalWrite(IRPT_WHEEL_PIN, HIGH); - delayMicroseconds(100); - digitalWrite(IRPT_WHEEL_PIN, LOW); - nextPulseTime_ms = now + pulseInterval_ms; - } -} - -// ===== Send Wheel Angle Sensor Values to DBW ===== -void sendAngleSensor(int angleT) { - int lSense, rSense; - if (angleT >= 0) { - lSense = L_STRAIGHT + (L_MIN - L_STRAIGHT) * angleT / MAX_ANGLE_TENTHS; - rSense = R_STRAIGHT + (R_MIN - R_STRAIGHT) * angleT / MAX_ANGLE_TENTHS; - } else { - lSense = L_STRAIGHT + (L_MAX - L_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; - rSense = R_STRAIGHT + (R_MAX - R_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; - } - analogWrite(L_SENSE_PIN, lSense * 4); - analogWrite(R_SENSE_PIN, rSense * 4); -} +// computeSpeed, updateAngle, updatePosition, sendWheelPulse, sendAngleSensor +// are defined in ../simulator_physics.h. From 01b6bad2686318ddf9ceadbcbe55d2b39ec4a02a Mon Sep 17 00:00:00 2001 From: toprakcelikel Date: Tue, 9 Jun 2026 03:23:57 -0700 Subject: [PATCH 2/3] Add PC-driven CSV test runner for closed-loop simulator Adds Sensor_Hub-as-gateway firmware mode plus a PC-side Python runner + scorer that exercises the full Sensor_Hub -> CAN -> DBW -> Router loop against a Stage-2 compatible CSV format extended with "# assert" lines. Files: - tools/test_runner.py: schedules CMD lines over USB, captures LOG telemetry from Sensor_Hub, evaluates assertions, prints PASS/FAIL, exits non-zero on any failure. - tools/sensorhub_runner_additions.md: protocol + Navigate.ino patch. - tools/proposed_assertions_test_steering_comprehensive.md: proposal for extending one of the existing Stage-2 CSVs with assertions. - tests/test_steering_response.csv: demo test, 4 commands + 4 assertions, validates closed-loop steering through DBW. - Navigate.ino: test-mode latch (useTestCommand), readTestCommand, publishTestCommand, printTestLog. Snoops 0x430 SimSteerActual as a second witness; falls through to autonomous waypoint nav if no CMD arrives. Verified end-to-end with all three Dues on the bus: 4 passed, 0 failed. --- NavigateTestRunner/NavigateTestRunner.ino | 218 ++++++++++++++++ tests/test_steering_response.csv | 37 +++ ..._assertions_test_steering_comprehensive.md | 114 ++++++++ tools/test_runner.py | 246 ++++++++++++++++++ 4 files changed, 615 insertions(+) create mode 100644 NavigateTestRunner/NavigateTestRunner.ino create mode 100644 tests/test_steering_response.csv create mode 100644 tools/proposed_assertions_test_steering_comprehensive.md create mode 100644 tools/test_runner.py diff --git a/NavigateTestRunner/NavigateTestRunner.ino b/NavigateTestRunner/NavigateTestRunner.ino new file mode 100644 index 0000000..42d513d --- /dev/null +++ b/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,,,,,\n + * Outbound (SH -> PC): ACK,\n + * LOG,,,...\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 + +// ===== 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,,,,, 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); +} diff --git a/tests/test_steering_response.csv b/tests/test_steering_response.csv new file mode 100644 index 0000000..5b5c5e0 --- /dev/null +++ b/tests/test_steering_response.csv @@ -0,0 +1,37 @@ +# Test: closed-loop steering response (through DBW) +# +# Format: time_ms, CANID, nbytes, speed_cmPs, brake, mode, angle_tenths +# - Backwards compatible with the existing Stage 2 CSV format. +# - `# assert t=: ` lines are evaluated by tools/test_runner.py +# against the SensorHub LOG telemetry stream. +# +# Wire path under test: +# PC -> SensorHub USB (CMD,350,...) +# SensorHub publishes 0x350 NavDrive + 0x100 NavStatus on CAN +# DBW PID reads 0x350 -> drives L_TURN / R_TURN pins +# Router observes pins -> integrates angle_tenths +# Router publishes 0x430 SimSteerActual -> DBW reads -> emits 0x400 +# SensorHub captures 0x400 int16[2] as actual_angle_tenths -> LOG line +# +# Slew expectation: +# Router: ANGLE_CHANGE_TENTHS=20 per 100ms loop => ~200 deg*10 / sec +# Full-lock-to-full-lock (~420 deg*10): ~2.1s of pure Router slew +# + DBW PID latency + CAN propagation -> samples at least 800ms after +# the last commanded transition to give the loop time to settle. + +# Cruise straight; baseline check. +0,350,6,1500,0,1,0 +# assert t=900: -20 <= actual_angle_tenths <= 20 + +# Command full right (+25 deg = 250 tenths). Slew is 200 deg*10/sec, so +# 0 -> 250 takes 1.25 s. +1000,350,6,1500,0,1,250 +# assert t=2800: 200 <= actual_angle_tenths <= 250 + +# Command full left (-25 deg). Full-lock-to-full-lock = 500/200 = 2.5 s. +4000,350,6,1500,0,1,-250 +# assert t=6700: -250 <= actual_angle_tenths <= -200 + +# Return to center. 250 -> 0 takes 1.25 s. +8000,350,6,1500,0,1,0 +# assert t=9800: -20 <= actual_angle_tenths <= 20 diff --git a/tools/proposed_assertions_test_steering_comprehensive.md b/tools/proposed_assertions_test_steering_comprehensive.md new file mode 100644 index 0000000..1a471ba --- /dev/null +++ b/tools/proposed_assertions_test_steering_comprehensive.md @@ -0,0 +1,114 @@ +# Proposed assertions for `test_steering_comprehensive.csv` + +A proposal — not a code change — showing how `# assert` lines could be +added to the existing `Non_grapic_simulator/test_steering_comprehensive.csv` +so the test produces pass/fail when run through `tools/test_runner.py`. + +## Why this file + +- Single axis (steering only; speed held at 15 m/s throughout). Easier to + reason about than the brake or combined tests. +- Already covers half-lock, full-lock, and return to center in both + directions — the full envelope of the closed-loop steering control. +- Closed-loop behavior is the headline change in the recent + `closed-loop-steering` / `full-sim-loop` PRs, so this is the natural + first test to make scorable. + +## Expected behavior the assertions should capture + +In through-DBW mode, when Nav publishes `0x350` with a target angle, +two things happen: + +1. **DBW's steering PID** drives `L_TURN` / `R_TURN` whenever the + measured angle is outside the deadband (`±5 deg×10`) of the target. +2. **Router** integrates `angle_tenths` by `ANGLE_CHANGE_TENTHS=20` each + 100 ms loop while a direction wire is asserted, until the deadband is + reached and DBW stops driving. + +Slew is therefore ~200 deg×10 / second. End-to-end latency from +"Nav publishes 0x350" → "Router's angle responds" is a few hundred ms +(Sensor_Hub loop period + DBW PID period + CAN propagation). So +assertions should sample **at least ~1 s after the command of interest** +to give the loop time to settle. + +The assertions reference `actual_angle_tenths`, which the Sensor_Hub +runner LOG line populates from DBW's `0x400` Actual frame (the value +DBW's PID is closing against). + +## Proposed file + +(File with `# assert` lines spliced in. Existing content unchanged.) + +```csv +# Comprehensive Steering Test +# Format: time_ms, CANID, nbytes, speed_cmPs, brake, mode, angle_tenths +# Description: Test left turn, right turn, and return to straight + +# Start: Turn left gradually +0,350,6,1500,0,1,-105 +500,350,6,1500,0,1,-210 +1000,350,6,1500,0,1,-210 +# assert t=1800: -230 <= actual_angle_tenths <= -190 + +# Return to straight +2000,350,6,1500,0,1,0 +2500,350,6,1500,0,1,0 +# assert t=3400: -20 <= actual_angle_tenths <= 20 + +# Turn right gradually +3500,350,6,1500,0,1,105 +4000,350,6,1500,0,1,210 +4500,350,6,1500,0,1,210 +# assert t=5300: 190 <= actual_angle_tenths <= 230 + +# Return to straight +5500,350,6,1500,0,1,0 +6000,350,6,1500,0,1,0 +# assert t=6900: -20 <= actual_angle_tenths <= 20 +``` + +## What each assertion checks + +| t (ms) | Range | What it verifies | +|--------|-------|------------------| +| 1800 | `[-230, -190]` | After ~1.3 s of commanding full left (-210), the modeled wheel angle has settled within ±20 of the target. ~200 ms slack on top of the theoretical slew time. | +| 3400 | `[-20, +20]` | After ~1.4 s of commanding centered (0), the wheel has returned through the deadband. ±20 covers the steering deadband (5) plus a few-loop transit. | +| 5300 | `[+190, +230]` | Symmetric right-side check of the same convergence behavior. | +| 6900 | `[-20, +20]` | Final return-to-center after the right-side maneuver. Closes the test. | + +## Tolerance rationale + +- **±20 deg×10 (=2°)** is generous on purpose for a first cut. The + deadband alone is ±5, so any settled state is within ±5 of the target + by construction. Padding to ±20 covers per-loop overshoot during the + PID's last 1-2 cycles plus measurement quantization in the 0x430 + feedback frame. Tighter once the test runs reliably. +- The **800 ms window between final command and assertion** is the + expected slew time (full-left → full-right is 2.1 s of pure Router + slew, plus PID/CAN latency) rounded up to give the loop time to + settle even on a slow Nav-loop tick. + +## Failure modes the assertions catch + +- **Deadband regression on DBW.** If the steering deadband gets widened + (e.g. someone bumps `DEADBAND_DegX10` from 5 to 30), the wheel would + stop short of the target and the ±20 tolerance would catch it. +- **Steering direction inverted** (L/R pins swapped). Commanded -210 + would produce +210, blowing the first assertion. +- **0x350 not arriving at DBW.** Router's wheel never moves; `actual_angle_tenths` + stays at 0, first assertion fails. +- **0x430 feedback missing.** DBW's open-loop fallback kicks in but + models the wrong rate; the wheel-vs-CAN values diverge over the test + and at least one assertion drifts out of tolerance. +- **PID integral runaway.** Wheel overshoots ±230 and the assertion's + upper bound catches it. + +## How to apply + +Not directly — propose the asserts as a PR against the file's owner. +The same scorer works against the file with or without the asserts; this +proposal is additive. + +If a future PR splices in these asserts, the test becomes the first +through-DBW closed-loop check in the project that produces machine-readable +pass/fail. diff --git a/tools/test_runner.py b/tools/test_runner.py new file mode 100644 index 0000000..2f3cbb9 --- /dev/null +++ b/tools/test_runner.py @@ -0,0 +1,246 @@ +#!/usr/bin/env python3 +""" +Elcano test runner — PC-driven CSV test for the closed-loop simulator. + +Reads a test CSV (Stage 2 command rows + `# assert` lines), pushes +commands to Sensor_Hub over USB serial at the scheduled times, captures +the LOG telemetry Sensor_Hub prints back, evaluates the assertions, and +prints a pass/fail summary. + +Usage: + python test_runner.py + +Examples: + python test_runner.py ../tests/test_steering_response.csv COM22 + python test_runner.py ../tests/test_steering_response.csv /dev/ttyACM0 + +CSV format (extends the existing Stage 2 CSVs — backwards compatible): + Command row: time_ms, CANID, nbytes, speed_cmPs, brake, mode, angle_tenths + Assertion line: # assert t=: + Comment: # any other line starting with # + +Sensor_Hub serial protocol (matching the NavigateTestRunner sketch in +NavigateTestRunner/NavigateTestRunner.ino): + Inbound (PC -> SH): CMD,,,,,\\n + Outbound (SH -> PC): LOG,,=,=,...\\n + plus ACK,\\n per accepted CMD + +Dependencies: + pip install pyserial +""" + +from __future__ import annotations + +import re +import sys +import time +import threading +from dataclasses import dataclass + +try: + import serial +except ImportError: + sys.stderr.write("Missing pyserial — install with: pip install pyserial\n") + sys.exit(2) + + +# -- Patterns ---------------------------------------------------------------- + +CMD_PATTERN = re.compile( + r"^\s*(\d+)\s*,\s*(\d+)\s*,\s*(\d+)\s*," + r"\s*(-?\d+)\s*,\s*(-?\d+)\s*,\s*(-?\d+)\s*,\s*(-?\d+)\s*$" +) +ASSERT_PATTERN = re.compile(r"^\s*#\s*assert\s+t=(\d+)\s*:\s*(.+?)\s*$") +LOG_PATTERN = re.compile(r"^LOG,(\d+),(.+)$") + + +# -- Data -------------------------------------------------------------------- + +@dataclass +class Command: + time_ms: int + can_id: int + speed: int + brake: int + mode: int + angle: int + + +@dataclass +class Assertion: + time_ms: int + expression: str + + +# -- CSV parsing ------------------------------------------------------------- + +def parse_csv(path: str) -> tuple[list[Command], list[Assertion]]: + commands: list[Command] = [] + asserts: list[Assertion] = [] + with open(path) as f: + for raw in f: + line = raw.rstrip("\r\n") + stripped = line.strip() + if not stripped: + continue + m = ASSERT_PATTERN.match(line) + if m: + asserts.append(Assertion(int(m.group(1)), m.group(2))) + continue + if stripped.startswith("#"): + continue + m = CMD_PATTERN.match(line) + if m: + commands.append( + Command( + time_ms=int(m.group(1)), + can_id=int(m.group(2)), + speed=int(m.group(4)), + brake=int(m.group(5)), + mode=int(m.group(6)), + angle=int(m.group(7)), + ) + ) + else: + sys.stderr.write(f" [warn] unparseable line: {line!r}\n") + commands.sort(key=lambda c: c.time_ms) + asserts.sort(key=lambda a: a.time_ms) + return commands, asserts + + +# -- Serial reader thread ---------------------------------------------------- + +class LogReader(threading.Thread): + """Background thread: drain serial, capture LOG lines, echo others.""" + + def __init__(self, ser: "serial.Serial", t0: float, verbose: bool = False): + super().__init__(daemon=True) + self.ser = ser + self.t0 = t0 + self.verbose = verbose + self.log: list[tuple[int, dict[str, int]]] = [] + self._stop_flag = threading.Event() + + def stop(self) -> None: + self._stop_flag.set() + + def run(self) -> None: + while not self._stop_flag.is_set(): + try: + raw = self.ser.readline() + except Exception: + continue + if not raw: + continue + line = raw.decode("ascii", errors="ignore").rstrip("\r\n") + if not line: + continue + elapsed_ms = int((time.monotonic() - self.t0) * 1000) + m = LOG_PATTERN.match(line) + if m: + # Index LOG entries by PC-side elapsed time, not the device's + # millis() — the device has been running long before the test + # started, so its millis values won't match our assertion times. + try: + fields: dict[str, int] = {} + for kv in m.group(2).split(","): + if "=" in kv: + k, v = kv.split("=", 1) + fields[k.strip()] = int(v.strip()) + self.log.append((elapsed_ms, fields)) + if self.verbose: + print(f" [{elapsed_ms:6d} ms] {line}") + except Exception: + pass + else: + # Echo human-readable lines as they happen + print(f" [{elapsed_ms:6d} ms] {line}") + + +# -- Assertion evaluation ---------------------------------------------------- + +def evaluate(assertion: Assertion, log: list[tuple[int, dict[str, int]]]) -> tuple[bool, str]: + if not log: + return False, "no LOG lines captured" + nearest = min(log, key=lambda e: abs(e[0] - assertion.time_ms)) + drift_ms = nearest[0] - assertion.time_ms + if abs(drift_ms) > 500: + return False, f"no LOG within 500 ms of t={assertion.time_ms} (nearest: t={nearest[0]})" + fields = nearest[1] + # Substitute field names with values in the expression. + expr = assertion.expression + for k in sorted(fields.keys(), key=len, reverse=True): + expr = re.sub(rf"\b{re.escape(k)}\b", str(fields[k]), expr) + try: + result = eval(expr, {"__builtins__": {}}, {}) # noqa: S307 — restricted builtins + except Exception as e: + return False, f"eval error: {e!r} (expanded expr: {expr!r})" + return bool(result), f"@ t={nearest[0]} (Δ {drift_ms:+d} ms) → {expr}" + + +# -- Main -------------------------------------------------------------------- + +def main() -> int: + args = [a for a in sys.argv[1:] if not a.startswith("--")] + verbose = any(a in ("--verbose", "-v") for a in sys.argv[1:]) + if len(args) != 2: + sys.stderr.write(f"Usage: {sys.argv[0]} [--verbose] \n") + return 2 + csv_path, port = args[0], args[1] + + commands, asserts = parse_csv(csv_path) + print(f"Loaded {len(commands)} commands and {len(asserts)} assertions from {csv_path}") + + ser = serial.Serial(port, 115200, timeout=0.1) + # Let the Due settle (Native USB CDC takes a moment to come up after open). + time.sleep(0.5) + + t0 = time.monotonic() + reader = LogReader(ser, t0, verbose=verbose) + reader.start() + + # Schedule and send commands. + for cmd in commands: + target = t0 + cmd.time_ms / 1000.0 + delay = target - time.monotonic() + if delay > 0: + time.sleep(delay) + line = f"CMD,{cmd.can_id},{cmd.speed},{cmd.brake},{cmd.mode},{cmd.angle}\n" + ser.write(line.encode("ascii")) + ser.flush() + elapsed_ms = int((time.monotonic() - t0) * 1000) + print(f" [{elapsed_ms:6d} ms] -> {line.strip()}") + + # Wait until the last assertion's time + slack so all LOG lines arrive. + if asserts: + slack_s = max(a.time_ms for a in asserts) / 1000.0 + 1.0 + target = t0 + slack_s + delay = target - time.monotonic() + if delay > 0: + time.sleep(delay) + + reader.stop() + reader.join(timeout=1) + ser.close() + + # Evaluate. + print() + print("=" * 60) + print(f"Assertions ({len(asserts)}):") + passed = failed = 0 + for a in asserts: + ok, detail = evaluate(a, reader.log) + status = "PASS" if ok else "FAIL" + print(f" [{status}] t={a.time_ms:>5d} ms: {a.expression}") + print(f" {detail}") + if ok: + passed += 1 + else: + failed += 1 + print() + print(f"Summary: {passed} passed, {failed} failed, {len(reader.log)} LOG samples captured") + return 0 if failed == 0 else 1 + + +if __name__ == "__main__": + sys.exit(main()) From 8da87c0c282463b56f5e47d2d657472b27a661d0 Mon Sep 17 00:00:00 2001 From: toprakcelikel Date: Tue, 16 Jun 2026 15:09:12 -0700 Subject: [PATCH 3/3] Remove Stage 2 ASCII command and Stage 3 binary telemetry from closed-loop sim --- .../simulator_closed_loop.ino | 264 +++--------------- 1 file changed, 43 insertions(+), 221 deletions(-) diff --git a/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino b/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino index 12bcb9d..88f87da 100644 --- a/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino +++ b/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino @@ -3,18 +3,23 @@ * Router Arduino Due * * SISTER SKETCH to ../simulator_stage1/simulator_stage1.ino. - * simulator_stage1 : standalone / standalone-Due use, no CAN, simple - * test mode with fixed throttle. Owned by Minhee. + * simulator_stage1 : standalone / standalone-Due use, with a Stage 2 + * PC-driven ASCII command path and the Stage 3 + * binary USB telemetry pipe. Owned by Minhee. * simulator_closed_loop: end-to-end closed-loop test on the Bridge board. * Reads real DBW inputs (throttle / L_TURN / R_TURN), * publishes pose + actual wheel angle on CAN so the * Sensor Hub Nav + DBW close their loops through it. * - * The two sketches share the same integer physics engine (sin1000, cos1000, - * computeSpeed, updateAngle, updatePosition, sendWheelPulse, - * sendAngleSensor). Those functions are mirrored here verbatim. If the - * physics changes in simulator_stage1.ino, mirror it here too (or factor - * out to a shared header later). + * This sketch deliberately omits the PC-driven Stage 2 ASCII command path + * and the Stage 3 binary USB telemetry packets — those belong in + * simulator_stage1.ino, where they have a use case (driving Router without + * DBW, or talking to the older Pygame PC simulator). The closed-loop sketch + * only ever runs on a Bridge board with the real DBW and Sensor Hub + * present; commands arrive on CAN and pose goes out on CAN. + * + * Both sketches share the same integer physics engine via the in-repo + * library at ../libraries/simulator_physics/. * * --- Hardware I/O --- * Inputs from DBW Arduino (via Bridge harness): @@ -39,12 +44,6 @@ * measured wheel angle for its PID. On the real trike * DBW would read its own analog sensors instead. * - * --- Stage 2 ASCII command pipe (PC -> Router via USB) --- - * Format: "CANID,nbytes,data1,data2,...\n" - * Example: "350,6,1500,0,1,-21" -> CAN ID 0x350, speed=1500 cm/s, brake=0, - * mode=1, angle=-21 (=-2.1 deg left). Lets a PC script drive Router - * without DBW being present. - * * Logs CSV to SD card if available, falls back to USB Serial (LOG_PORT). * CSV: time_ms, X_mm, Y_mm, heading_tenths, speed_mmPs, angle_tenths, throttle, brakeOn * @@ -68,8 +67,9 @@ #include // ===== Logging / USB Serial port ===== -// LOG_PORT is used for ALL host-side I/O: CSV log, debug prints, Stage 2 -// ASCII command input, Stage 3 binary 0x251/0x4C0 packets. +// LOG_PORT is used for diagnostic CSV log and human-readable debug prints +// only — this sketch does not accept PC commands and does not emit binary +// telemetry packets. // // USE_NATIVE_USB defined (default for this sketch) -> SerialUSB (Native USB port) // USE_NATIVE_USB commented out -> Serial (Programming port) @@ -114,8 +114,6 @@ // ===== CAN ID Definitions ===== // Per https://www.elcanoproject.org/wiki/Communication -// Nav->DBW: speed(cm/s), brake, mode, angle(deg x10) -#define CAN_DRIVE 0x350 // Router->Sensor Hub: vehicle position east_cm, north_cm #define CAN_POSITION 0x4C0 // Router->Sensor Hub: heading_centiDeg (compass only) @@ -126,18 +124,6 @@ // Substitutes for L_SENSE/R_SENSE analog feedback that is unavailable on // this bridge. On the real trike, DBW would read its analog sensors. #define CAN_STEER_ACTUAL 0x430 -// Nav->all: GPS origin lat/lon -#define CAN_ORIGIN 0x251 - -// ===== Origin GPS coordinates (UW Bothell) ===== -// latitude degrees -#define ORIGIN_LAT 47 -// latitude fraction (0-9,999,999) -#define ORIGIN_LAT_FRAC 760934 -// longitude degrees -#define ORIGIN_LON 122 -// longitude fraction (0-999,999), West -#define ORIGIN_LON_FRAC 189963 // ===== Speed Model Constants ===== #define FRICTION_NUM 9296 @@ -179,27 +165,13 @@ long Y_mm = 0; unsigned long nextPulseTime_ms = 0; -// ===== Stage 2: ASCII command state ===== -bool useScriptCommand = false; -// commanded speed from PC (cm/s) -int cmd_speed_cmPs = 0; -// 0=off, 1=hold(12V), 2=on(24V) -int cmd_brake = 0; -// 0=init,1=RC,2=operator,3=auto-RC,4=auto-op -int cmd_mode = 0; -// commanded steer angle (degrees x10) -int cmd_angle_tenths = 0; - // ===== SD Card Variables ===== File logFile; bool sdAvailable = false; // ===== Function Declarations ===== // sin1000, cos1000, computeSpeed, updateAngle, updatePosition, -// sendWheelPulse, sendAngleSensor are provided by ../simulator_physics.h. -void readScriptCommand(); -void sendPosition(); -void sendOrigin(); +// sendWheelPulse, sendAngleSensor are provided by simulator_physics.h. bool sendPositionCAN(); bool sendHeadingCAN(); bool sendSpeedCAN(); @@ -257,11 +229,6 @@ void setup() { LOG_PORT.println("CAN init FAILED"); } - // Stage 3 binary USB origin (0x251) intentionally not sent at boot — - // Sensor Hub Nav reads pose over CAN (0x4C0/0x4E0/0x4F0). The - // sendOrigin() function is kept defined for backwards compat with the - // older PC simulator pipe. - // Initialize SD card LOG_PORT.print("Initializing SD card on pin D"); LOG_PORT.print(SD_CS_PIN); @@ -305,55 +272,35 @@ void loop() { // the whole drive. uint32_t startTime = millis(); - // Stage 2: read ASCII command from PC if available - readScriptCommand(); - - int throttle; - bool brakeOn; - bool lTurn; - bool rTurn; - - if (useScriptCommand) { - // Drive command received from PC simulator (Stage 2). - // speed and angle are set directly; skip throttle model. - // cm/s -> mm/s - speed_mmPs = cmd_speed_cmPs * 10; - angle_tenths = cmd_angle_tenths; - brakeOn = (cmd_brake > 0); - throttle = 0; - lTurn = false; - rTurn = false; - } else { - // Real inputs from DBW (closes the loop: Nav -> CAN -> DBW -> pins -> here). - // THROTTLE_PIN (A0) <- DBW DAC0 throttle output, 0-1023 analog - // BRAKE_ON_PIN (D48) <- DBW BRAKE_ON_PIN (D44) - // L_TURN_PIN (D4) <- DBW LEFT_TURN_PIN (D26): HIGH = turn left - // R_TURN_PIN (D2) <- DBW RIGHT_TURN_PIN (D28): HIGH = turn right - int rawThrottle = analogRead(THROTTLE_PIN); - throttle = rawThrottle / 4; - // BRAKE derived from throttle below the effective threshold. When Nav - // commands speed=0, DBW's throttle PID outputs 0 and we treat that as brake. - brakeOn = (throttle < MIN_EFFECTIVE_THROTTLE); - - // Steering via two digital wires from DBW (L_TURN D4, R_TURN D2). - lTurn = (digitalRead(L_TURN_PIN) == HIGH); - rTurn = (digitalRead(R_TURN_PIN) == HIGH); - angle_tenths = updateAngle(lTurn, rTurn); - - static int dbgCount = 0; - if (++dbgCount >= 10) { - dbgCount = 0; - LOG_PORT.print("# L="); LOG_PORT.print(lTurn ? 1 : 0); - LOG_PORT.print(" R="); LOG_PORT.print(rTurn ? 1 : 0); - LOG_PORT.print(" ang_tenths="); LOG_PORT.print(angle_tenths); - LOG_PORT.print(" speed="); LOG_PORT.println(speed_mmPs); - } - - throttleHistory[historyIndex] = throttle; - historyIndex = (historyIndex + 1) % THROTTLE_HISTORY; - speed_mmPs = computeSpeed(throttle, brakeOn); + // Real inputs from DBW (closes the loop: Nav -> CAN -> DBW -> pins -> here). + // THROTTLE_PIN (A0) <- DBW DAC0 throttle output, 0-1023 analog + // BRAKE_ON_PIN (D48) <- DBW BRAKE_ON_PIN (D44) + // L_TURN_PIN (D4) <- DBW LEFT_TURN_PIN (D26): HIGH = turn left + // R_TURN_PIN (D2) <- DBW RIGHT_TURN_PIN (D28): HIGH = turn right + int rawThrottle = analogRead(THROTTLE_PIN); + int throttle = rawThrottle / 4; + // BRAKE derived from throttle below the effective threshold. When Nav + // commands speed=0, DBW's throttle PID outputs 0 and we treat that as brake. + bool brakeOn = (throttle < MIN_EFFECTIVE_THROTTLE); + + // Steering via two digital wires from DBW (L_TURN D4, R_TURN D2). + bool lTurn = (digitalRead(L_TURN_PIN) == HIGH); + bool rTurn = (digitalRead(R_TURN_PIN) == HIGH); + angle_tenths = updateAngle(lTurn, rTurn); + + static int dbgCount = 0; + if (++dbgCount >= 10) { + dbgCount = 0; + LOG_PORT.print("# L="); LOG_PORT.print(lTurn ? 1 : 0); + LOG_PORT.print(" R="); LOG_PORT.print(rTurn ? 1 : 0); + LOG_PORT.print(" ang_tenths="); LOG_PORT.print(angle_tenths); + LOG_PORT.print(" speed="); LOG_PORT.println(speed_mmPs); } + throttleHistory[historyIndex] = throttle; + historyIndex = (historyIndex + 1) % THROTTLE_HISTORY; + speed_mmPs = computeSpeed(throttle, brakeOn); + // Update vehicle position updatePosition(speed_mmPs, heading_tenths, angle_tenths); @@ -400,73 +347,6 @@ void loop() { if (elapsed < LOOP_TIME_MS) delay(LOOP_TIME_MS - elapsed); } -// =========================================================================== -// Stage 2: ASCII drive command from PC via USB Serial -// =========================================================================== -// Format: "CANID,nbytes,data1,data2,...\n" -// Example: "350,6,1500,0,1,-21" -// CAN ID 0x350, 6 data bytes -// speed=1500 cm/s, brake=0, mode=1, angle=-21 (=-2.1 deg left) -// Optional execution time prefix (ms): "1000,350,6,1500,0,1,-21" -// Lines starting with '#' are comments and ignored. -void readScriptCommand() { - static char buf[64]; - static int bufIdx = 0; - - while (LOG_PORT.available()) { - char c = (char)LOG_PORT.read(); - if (c == '\n' || c == '\r') { - buf[bufIdx] = '\0'; - bufIdx = 0; - - // Skip empty lines and comments - if (buf[0] == '\0' || buf[0] == '#') return; - - // Parse comma-separated integer fields - int fields[12]; - int nFields = 0; - char *ptr = buf; - while (*ptr && nFields < 12) { - fields[nFields++] = (int)strtol(ptr, &ptr, 0); - if (*ptr == ',') ptr++; - } - if (nFields < 3) return; - - // If first value > 0x7FF it is an execution time prefix; skip it - int idx = (fields[0] > 0x7FF) ? 1 : 0; - if (nFields < idx + 3) return; - - int canId = fields[idx]; - // fields[idx+1] = nbytes (not needed directly) - // speed (cm/s) - int d0 = (nFields > idx+2) ? fields[idx+2] : 0; - // brake - int d1 = (nFields > idx+3) ? fields[idx+3] : 0; - // mode - int d2 = (nFields > idx+4) ? fields[idx+4] : 0; - // angle (deg x10) - int d3 = (nFields > idx+5) ? fields[idx+5] : 0; - - if (canId == CAN_DRIVE) { - // 0x350: speed(cm/s), brake, mode, steer angle(deg x10) - cmd_speed_cmPs = d0; - cmd_brake = d1; - cmd_mode = d2; - cmd_angle_tenths = d3; - useScriptCommand = true; - // Debug echo so PC can verify Router received the command - LOG_PORT.print("CMD: speed="); LOG_PORT.print(cmd_speed_cmPs); - LOG_PORT.print(" brake="); LOG_PORT.print(cmd_brake); - LOG_PORT.print(" mode="); LOG_PORT.print(cmd_mode); - LOG_PORT.print(" angle="); LOG_PORT.println(cmd_angle_tenths); - } - // Future: handle other CAN IDs as needed - } else { - if (bufIdx < 63) buf[bufIdx++] = c; - } - } -} - // =========================================================================== // CAN frame senders (Router -> bus, called once per loop) // =========================================================================== @@ -524,61 +404,3 @@ bool sendSteerActualCAN() { f.data.s0 = (uint16_t)(int16_t)angle_tenths; return Can0.sendFrame(f); } - -// =========================================================================== -// Stage 3: Binary USB packets (kept for the older PC simulator pipe) -// =========================================================================== - -// 0x4C0 binary packet to PC via USB. Used by older PC sim that doesn't speak -// CAN; Sensor Hub Nav reads from sendPositionCAN above instead. -// Per wiki: Bytes 1-4 = east_cm (int32), Bytes 5-8 = north_cm (int32) -// Packet header: 2 bytes [ID_HIGH, ID_LOW|(len<<1)] -// 0x4C0 >> 3 = 0x98 -> ID_HIGH -// ((0x4C0 & 0x07) << 5) | (8 << 1) = 0x00 | 0x10 = 0x10 -> ID_LOW|len -void sendPosition() { - int32_t east_cm = (int32_t)(X_mm / 10); - int32_t north_cm = (int32_t)(Y_mm / 10); - - // CAN ID high byte - LOG_PORT.write(0x98); - // CAN ID low bits | data length - LOG_PORT.write(0x10); - // east_cm big-endian int32 - LOG_PORT.write((uint8_t)(east_cm >> 24)); - LOG_PORT.write((uint8_t)(east_cm >> 16)); - LOG_PORT.write((uint8_t)(east_cm >> 8)); - LOG_PORT.write((uint8_t)(east_cm )); - // north_cm big-endian int32 - LOG_PORT.write((uint8_t)(north_cm >> 24)); - LOG_PORT.write((uint8_t)(north_cm >> 16)); - LOG_PORT.write((uint8_t)(north_cm >> 8)); - LOG_PORT.write((uint8_t)(north_cm )); -} - -// 0x251 GPS origin binary packet. Kept for backwards compat; not called at -// boot in this sketch. -// Per wiki Set Origin (0x251): -// Byte 1: lat degrees (7 bits) + N/S (bit 8, 0=N) -// Bytes 2,3,4: lat fraction (0-9,999,999) -// Byte 5: lon degrees -// Byte 6 bit 1: E/W (0=E, 1=W); rest of byte 6 + bytes 7,8: lon fraction -void sendOrigin() { - // Packet header for 0x251, 8 data bytes - // 0x251 >> 3 = 0x4A -> ID_HIGH - // ((0x251 & 0x07) << 5) | (8 << 1) = 0x20 | 0x10 = 0x30 -> ID_LOW|len - LOG_PORT.write(0x4A); - LOG_PORT.write(0x30); - // Byte 1: lat degrees, N hemisphere -> bit 8 = 0 - LOG_PORT.write((uint8_t)ORIGIN_LAT); - // Bytes 2,3,4: lat fraction = 760934 - LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC >> 16)); - LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC >> 8)); - LOG_PORT.write((uint8_t)(ORIGIN_LAT_FRAC )); - // Byte 5: lon degrees = 122 - LOG_PORT.write((uint8_t)ORIGIN_LON); - // Bytes 6,7,8: W -> first bit of byte 6 = 1, fraction = 189963 - uint32_t lon_field = (1UL << 23) | (uint32_t)ORIGIN_LON_FRAC; - LOG_PORT.write((uint8_t)(lon_field >> 16)); - LOG_PORT.write((uint8_t)(lon_field >> 8)); - LOG_PORT.write((uint8_t)(lon_field )); -}