diff --git a/Non_grapic_simulator/simulator_stage1.ino b/Non_grapic_simulator/simulator_stage1.ino index f24654a..72df251 100644 --- a/Non_grapic_simulator/simulator_stage1.ino +++ b/Non_grapic_simulator/simulator_stage1.ino @@ -1,9 +1,9 @@ /* - * Simulator Stage 1 + * Simulator Stage 1 + Stage 3 * Router Arduino Due * - * Reads inputs from DBW Arduino: - * A0 -> THROTTLE + * Reads inputs from DBW Arduino OR from PC via USB Serial (Stage 2 simulator): + * A0 -> THROTTLE (from DBW, if connected) * D42 -> BRAKE_VOLT * D48 -> BRAKE_ON * D4 -> L_TURN @@ -14,12 +14,29 @@ * DAC0 -> L_SENSE (left wheel angle) * DAC1 -> R_SENSE (right wheel angle) * + * Stage 2 integration (PC -> Router): + * Reads ASCII commands from USB Serial sent by PC simulator. + * Format: "CANID,nbytes,data1,data2,...\n" + * Example: "350,6,1500,0,1,-21" + * CANID=350, nbytes=6 + * data: speed=1500 cm/s, brake=0, mode=1, angle=-21 (=-2.1 deg) + * Lines starting with '#' are comments and ignored. + * Optional execution time prefix: "1000,350,6,1500,0,1,-21" + * + * Stage 3 GPS output (Router -> PC): + * Sends binary packets over USB Serial every loop. + * 0x251: GPS origin (UW Bothell) once at startup. + * 0x4C0: Vehicle position (east_cm, north_cm) every 100ms. + * Binary format per professor's instruction: + * same data as CAN, encoding is binary. + * * Logs CSV to SD card if available, falls back to Serial. * CSV: time_ms, X_mm, Y_mm, heading_tenths, speed_mmPs, angle_tenths, throttle, brakeOn * * All arithmetic uses integers (no float) per professor's requirement. - * Angles are stored as tenths of a degree (e.g. 255 = 25.5 deg). - * Positions are in mm. Speed is in mm/s. + * Angles stored as tenths of a degree. Positions in mm. Speed in mm/s. + * + * Reference: https://www.elcanoproject.org/wiki/Communication */ #include @@ -36,51 +53,65 @@ #define R_SENSE_PIN DAC1 // To DBW A11 // ===== SD Card Pin ===== -// Connect Sensor Hub Pin 35 to 37 (router pin D20) #define SD_CS_PIN 37 +// ===== CAN ID Definitions ===== +// Per https://www.elcanoproject.org/wiki/Communication +#define CAN_DRIVE 0x350 // Nav->DBW: speed(cm/s), brake, mode, angle(deg x10) +#define CAN_POSITION 0x4C0 // Nav->all: vehicle position east_cm, north_cm +#define CAN_ORIGIN 0x251 // Nav->all: GPS origin lat/lon + +// ===== Origin GPS coordinates (UW Bothell) ===== +#define ORIGIN_LAT 47 // latitude degrees +#define ORIGIN_LAT_FRAC 760934 // latitude fraction (0-9,999,999) +#define ORIGIN_LON 122 // longitude degrees +#define ORIGIN_LON_FRAC 189963 // longitude fraction (0-999,999), West + // ===== Speed Model Constants ===== -// FRICTION = 0.9296 represented as 9296/10000 #define FRICTION_NUM 9296 #define FRICTION_DEN 10000 #define MIN_EFFECTIVE_THROTTLE 65 #define MAX_EFFECTIVE_THROTTLE 227 -#define MAX_SPEED_mmPs 13600 // 13.6 m/s in mm/s -#define THROTTLE_HISTORY 10 // Number of throttle samples to keep -#define THROTTLE_DELAY_START 3 // Start index for mean (from t-10) -#define THROTTLE_DELAY_END 10 // End index for mean (to t-3, 8 samples) +#define MAX_SPEED_mmPs 13600 +#define THROTTLE_HISTORY 10 +#define THROTTLE_DELAY_START 3 +#define THROTTLE_DELAY_END 10 // ===== Vehicle Settings ===== -#define WHEEL_DIAMETER_MM 495 // mm (rounded from 495.3) -#define WHEEL_CIRCUM_MM 1555 // mm (495 * pi, rounded) -#define LOOP_TIME_MS 100 // Loop period in ms -#define MAX_ANGLE_TENTHS 250 // 25.0 degrees in tenths -#define ANGLE_CHANGE_TENTHS 20 // 2.0 degrees per loop in tenths +#define WHEEL_DIAMETER_MM 495 +#define WHEEL_CIRCUM_MM 1555 +#define LOOP_TIME_MS 100 +#define MAX_ANGLE_TENTHS 250 +#define ANGLE_CHANGE_TENTHS 20 // ===== Wheel Angle Sensor ===== -// L_SENSE: Straight=722, Min(hard right)=779, Max(hard left)=639 #define L_STRAIGHT 722 #define L_MIN 779 #define L_MAX 639 -// R_SENSE: Straight=731, Min(hard right)=673, Max(hard left)=786 #define R_STRAIGHT 731 #define R_MIN 673 #define R_MAX 786 // ===== Global Variables (all integer) ===== -int throttleHistory[THROTTLE_HISTORY]; // Ring buffer of recent throttle values +int throttleHistory[THROTTLE_HISTORY]; int historyIndex = 0; -int speed_mmPs = 0; // Current speed in mm/s -int prevSpeed_mmPs = 0; // Previous speed in mm/s +int speed_mmPs = 0; +int prevSpeed_mmPs = 0; +int heading_tenths = 0; +int angle_tenths = 0; -int heading_tenths = 0; // Vehicle heading in tenths of degree (0=North, 900=East) -int angle_tenths = 0; // Steering angle in tenths of degree (negative=left, positive=right) +long X_mm = 0; +long Y_mm = 0; -long X_mm = 0; // East-West position in mm -long Y_mm = 0; // North-South position in mm +unsigned long nextPulseTime_ms = 0; -unsigned long nextPulseTime_ms = 0; // Timestamp of next wheel pulse +// ===== Stage 2: ASCII command state ===== +bool useScriptCommand = false; +int cmd_speed_cmPs = 0; // commanded speed from PC (cm/s) +int cmd_brake = 0; // 0=off, 1=hold(12V), 2=on(24V) +int cmd_mode = 0; // 0=init,1=RC,2=operator,3=auto-RC,4=auto-op +int cmd_angle_tenths = 0; // commanded steer angle (degrees x10) // ===== SD Card Variables ===== File logFile; @@ -91,15 +122,14 @@ 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 angle_tenths); +void sendAngleSensor(int angleT); +void readScriptCommand(); +void sendPosition(); +void sendOrigin(); // ===== Integer sine/cosine (returns value * 1000) ===== -// Uses lookup table for 0-90 degrees, maps other quadrants -// angle_tenths: angle in tenths of degree (0-3600) int sin1000(int angle_tenths) { - // Normalize to 0-3599 angle_tenths = ((angle_tenths % 3600) + 3600) % 3600; - // sin lookup table for 0-90 deg in 1-deg steps (* 1000) 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, @@ -136,10 +166,10 @@ void setup() { pinMode(IRPT_WHEEL_PIN, OUTPUT); digitalWrite(IRPT_WHEEL_PIN, LOW); - // Initialize throttle history buffer - for (int i = 0; i < THROTTLE_HISTORY; i++) { - throttleHistory[i] = 0; - } + for (int i = 0; i < THROTTLE_HISTORY; i++) throttleHistory[i] = 0; + + // Stage 3: send GPS origin once at startup (binary 0x251 packet) + sendOrigin(); // Initialize SD card Serial.print("Initializing SD card on pin D"); @@ -155,8 +185,6 @@ void setup() { sprintf(filename, "SIM%02d.CSV", i); if (!SD.exists(filename)) break; } - Serial.print("Opening file: "); - Serial.println(filename); logFile = SD.open(filename, FILE_WRITE); if (logFile) { sdAvailable = true; @@ -167,23 +195,19 @@ void setup() { } } else { Serial.println("SD not found. Using Serial."); - Serial.println("Check: JP2 jumpers x3, wire SH Pin35 to TP12(D20)"); } - // Print CSV header - // Units: time=ms, X/Y=mm, heading/angle=tenths of degree, speed=mm/s if (sdAvailable) { logFile.println("time_ms,X_mm,Y_mm,heading_tenths,speed_mmPs,angle_tenths,throttle,brakeOn"); logFile.flush(); } else { Serial.println("time_ms,X_mm,Y_mm,heading_tenths,speed_mmPs,angle_tenths,throttle,brakeOn"); } - Serial.println("Simulator Stage 1 started."); + Serial.println("Simulator Stage 1+3 started."); } // ===== Main Loop ===== void loop() { - // Stop after 50 seconds if (millis() > 50000) { if (sdAvailable) { logFile.println("Done."); @@ -198,39 +222,54 @@ void loop() { uint32_t startTime = millis(); - // Fixed test values (uncomment below to read from DBW pins) - int throttle = 150; - bool brakeOn = false; - bool brakeVolt = false; - bool lTurn = false; - bool rTurn = false; - - // Uncomment to read real inputs from DBW: - // int rawThrottle = analogRead(THROTTLE_PIN); - // int throttle = rawThrottle / 4; - // bool brakeOn = digitalRead(BRAKE_ON_PIN); - // bool brakeVolt = digitalRead(BRAKE_VOLT_PIN); - // bool lTurn = digitalRead(L_TURN_PIN); - // bool rTurn = digitalRead(R_TURN_PIN); - - // 2. Update throttle history buffer - throttleHistory[historyIndex] = throttle; - historyIndex = (historyIndex + 1) % THROTTLE_HISTORY; - - // 3. Compute speed - speed_mmPs = computeSpeed(throttle, brakeOn); - - // 4. Update steering angle - angle_tenths = updateAngle(lTurn, rTurn); + // Stage 2: read ASCII command from PC if available + readScriptCommand(); + + int throttle; + bool brakeOn; + bool lTurn; + bool rTurn; + + if (useScriptCommand) { + // Use drive command received from PC simulator (Stage 2) + // speed and angle are set directly; skip throttle model + speed_mmPs = cmd_speed_cmPs * 10; // cm/s -> mm/s + angle_tenths = cmd_angle_tenths; + brakeOn = (cmd_brake > 0); + throttle = 0; + lTurn = false; + rTurn = false; + } else { + // Fixed test values (uncomment below to read real DBW inputs) + throttle = 150; + brakeOn = false; + lTurn = false; + rTurn = false; + + // Uncomment to read real inputs from DBW: + // int rawThrottle = analogRead(THROTTLE_PIN); + // throttle = rawThrottle / 4; + // brakeOn = digitalRead(BRAKE_ON_PIN); + // lTurn = digitalRead(L_TURN_PIN); + // rTurn = digitalRead(R_TURN_PIN); + + throttleHistory[historyIndex] = throttle; + historyIndex = (historyIndex + 1) % THROTTLE_HISTORY; + speed_mmPs = computeSpeed(throttle, brakeOn); + angle_tenths = updateAngle(lTurn, rTurn); + } - // 5. Update vehicle position + // Update vehicle position updatePosition(speed_mmPs, heading_tenths, angle_tenths); - // 6. Send simulated sensor values to DBW + // Send simulated sensor values to DBW Arduino sendWheelPulse(speed_mmPs); sendAngleSensor(angle_tenths); - // 7. Log CSV line (all integers) + // Stage 3: send vehicle position to PC as binary 0x4C0 packet + sendPosition(); + + // Log CSV to SD card or Serial if (sdAvailable) { logFile.print(millis()); logFile.print(","); logFile.print(X_mm); logFile.print(","); @@ -252,80 +291,167 @@ void loop() { Serial.println(brakeOn ? 1 : 0); } - // 8. Wait to maintain 100ms loop period uint32_t elapsed = millis() - startTime; - if (elapsed < LOOP_TIME_MS) - delay(LOOP_TIME_MS - elapsed); + if (elapsed < LOOP_TIME_MS) delay(LOOP_TIME_MS - elapsed); +} + +// ===== Stage 2: Read 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 (Serial.available()) { + char c = (char)Serial.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) + int d0 = (nFields > idx+2) ? fields[idx+2] : 0; // speed (cm/s) + int d1 = (nFields > idx+3) ? fields[idx+3] : 0; // brake + int d2 = (nFields > idx+4) ? fields[idx+4] : 0; // mode + int d3 = (nFields > idx+5) ? fields[idx+5] : 0; // angle (deg x10) + + 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 print so PC can verify Router received the command + Serial.print("CMD: speed="); Serial.print(cmd_speed_cmPs); + Serial.print(" brake="); Serial.print(cmd_brake); + Serial.print(" mode="); Serial.print(cmd_mode); + Serial.print(" angle="); Serial.println(cmd_angle_tenths); + } + // Future: handle other CAN IDs as needed + } else { + if (bufIdx < 63) buf[bufIdx++] = c; + } + } +} + +// ===== Stage 3: Send vehicle position as binary 0x4C0 packet ===== +// 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); + + Serial.write(0x98); // CAN ID high byte + Serial.write(0x10); // CAN ID low bits | data length + // east_cm big-endian int32 + Serial.write((uint8_t)(east_cm >> 24)); + Serial.write((uint8_t)(east_cm >> 16)); + Serial.write((uint8_t)(east_cm >> 8)); + Serial.write((uint8_t)(east_cm )); + // north_cm big-endian int32 + Serial.write((uint8_t)(north_cm >> 24)); + Serial.write((uint8_t)(north_cm >> 16)); + Serial.write((uint8_t)(north_cm >> 8)); + Serial.write((uint8_t)(north_cm )); +} + +// ===== Stage 3: Send GPS origin as binary 0x251 packet ===== +// 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 +// Sent once at startup. +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 + Serial.write(0x4A); + Serial.write(0x30); + // Byte 1: lat degrees, N hemisphere -> bit 8 = 0 + Serial.write((uint8_t)ORIGIN_LAT); + // Bytes 2,3,4: lat fraction = 760934 + Serial.write((uint8_t)(ORIGIN_LAT_FRAC >> 16)); + Serial.write((uint8_t)(ORIGIN_LAT_FRAC >> 8)); + Serial.write((uint8_t)(ORIGIN_LAT_FRAC )); + // Byte 5: lon degrees = 122 + Serial.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; + Serial.write((uint8_t)(lon_field >> 16)); + Serial.write((uint8_t)(lon_field >> 8)); + Serial.write((uint8_t)(lon_field )); } // ===== Compute Speed (integer arithmetic) ===== int computeSpeed(int throttle, bool brakeOn) { if (brakeOn) { prevSpeed_mmPs = 0; return 0; } - - // Compute mean throttle over samples t-10 to t-3 (8 samples) 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); - - // Estimated speed from throttle model (mm/s) int estimatedSpeed = 0; - if (meanThrottle > MIN_EFFECTIVE_THROTTLE) { + 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; - - // Momentum: prevSpeed * 0.9296 = prevSpeed * 9296 / 10000 int momentum = (int)((long)prevSpeed_mmPs * FRICTION_NUM / FRICTION_DEN); - - // Final speed int newSpeed = (momentum > estimatedSpeed) ? momentum : estimatedSpeed; if (newSpeed > MAX_SPEED_mmPs) newSpeed = MAX_SPEED_mmPs; prevSpeed_mmPs = newSpeed; return newSpeed; } -// ===== Update Steering Angle (integer, tenths of degree) ===== +// ===== 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 (integer arithmetic) ===== +// ===== Update Vehicle Position ===== void updatePosition(int speed, int &heading, int angle) { - // heading change per loop: angle_tenths * 0.01 degrees - // = angle_tenths / 100 tenths = angle_tenths / 1000 * 10 tenths - if (speed > 0) { - heading += angle_tenths / 100; // rough heading change in tenths of degree - } + if (speed > 0) heading += angle / 10; if (heading >= 3600) heading -= 3600; if (heading < 0) heading += 3600; - - // Distance in mm per loop: speed_mmPs * 100ms / 1000 int distance_mm = speed * LOOP_TIME_MS / 1000; - - // Update X, Y using integer sin/cos (* 1000) - // X += distance * sin(heading) / 1000 - // Y += distance * cos(heading) / 1000 X_mm += (long)distance_mm * sin1000(heading) / 1000; Y_mm += (long)distance_mm * cos1000(heading) / 1000; } -// ===== Send Wheel Pulse ===== +// ===== Send Wheel Pulse to DBW ===== void sendWheelPulse(int speed_mmPs) { if (speed_mmPs <= 0) return; - - // Time for one full wheel revolution at current speed (ms) 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); @@ -335,20 +461,16 @@ void sendWheelPulse(int speed_mmPs) { } } -// ===== Send Wheel Angle Sensor Values ===== -void sendAngleSensor(int angle_tenths) { - // angle_tenths range: -250 to +250 (= -25.0 to +25.0 degrees) +// ===== Send Wheel Angle Sensor Values to DBW ===== +void sendAngleSensor(int angleT) { int lSense, rSense; - if (angle_tenths >= 0) { - // Right turn - lSense = L_STRAIGHT + (L_MIN - L_STRAIGHT) * angle_tenths / MAX_ANGLE_TENTHS; - rSense = R_STRAIGHT + (R_MIN - R_STRAIGHT) * angle_tenths / MAX_ANGLE_TENTHS; + 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 { - // Left turn - lSense = L_STRAIGHT + (L_MAX - L_STRAIGHT) * (-angle_tenths) / MAX_ANGLE_TENTHS; - rSense = R_STRAIGHT + (R_MAX - R_STRAIGHT) * (-angle_tenths) / MAX_ANGLE_TENTHS; + lSense = L_STRAIGHT + (L_MAX - L_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; + rSense = R_STRAIGHT + (R_MAX - R_STRAIGHT) * (-angleT) / MAX_ANGLE_TENTHS; } - // DAC output range: 0~4095 (12-bit) analogWrite(L_SENSE_PIN, lSense * 4); analogWrite(R_SENSE_PIN, rSense * 4); -} +} \ No newline at end of file