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 4d9c0f94dd62abf3c43c5b7c929e889df9fbdef6 Mon Sep 17 00:00:00 2001 From: toprakcelikel Date: Tue, 16 Jun 2026 15:09:12 -0700 Subject: [PATCH 2/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 )); -} From e1c49e5de878e3edeed4487293ec148ec1dceb85 Mon Sep 17 00:00:00 2001 From: toprakcelikel Date: Tue, 23 Jun 2026 16:57:02 -0700 Subject: [PATCH 3/3] Read DBW brake wire instead of inferring brake from throttle level Inferring brake from throttle < MIN_EFFECTIVE_THROTTLE falsely engaged the brake during low-speed PID ramp-up (e.g. Nav speed=100 cm/s), stalling the sim. Read BRAKE_ON_PIN (D48) directly; add BRAKE_ON_ACTIVE_LOW config (default true for Bridge active-low relay wiring) and brake state to the debug line. --- .../simulator_closed_loop.ino | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 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 88f87da..69a8261 100644 --- a/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino +++ b/Non_grapic_simulator/simulator_closed_loop/simulator_closed_loop.ino @@ -98,6 +98,9 @@ #define BRAKE_VOLT_PIN 42 // From DBW D44 #define BRAKE_ON_PIN 48 +// DBW relay logic on Bridge is active-low: LOW means brake engaged. +// Set to false only if your brake control wiring is active-high. +#define BRAKE_ON_ACTIVE_LOW true // 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 @@ -279,9 +282,13 @@ void loop() { // 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); + int brakePinRaw = digitalRead(BRAKE_ON_PIN); + // Use DBW's explicit brake command wire instead of inferring brake from + // throttle level. Inferring from throttle suppresses low-speed motion + // (e.g. Nav speed=100 cm/s) because throttle may stay < MIN_EFFECTIVE_THROTTLE + // during PID ramp-up even when brakes are released. + bool brakeOn = BRAKE_ON_ACTIVE_LOW ? (brakePinRaw == LOW) + : (brakePinRaw == HIGH); // Steering via two digital wires from DBW (L_TURN D4, R_TURN D2). bool lTurn = (digitalRead(L_TURN_PIN) == HIGH); @@ -293,6 +300,7 @@ void loop() { 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(" B="); LOG_PORT.print(brakeOn ? 1 : 0); LOG_PORT.print(" ang_tenths="); LOG_PORT.print(angle_tenths); LOG_PORT.print(" speed="); LOG_PORT.println(speed_mmPs); }