#include <Wire.h>
#include <math.h>
#include <HardwareSerial.h>
#include <stdio.h>
// Debug UART to the Wokwi Serial Monitor:
// RX=PA10, TX=PA9
HardwareSerial DebugSerial(PA10, PA9);
// Raspberry Pi vision UART:
// RX=PB11, TX=PB10
//
// Physical wiring:
// Pi TX (GPIO14 / pin 8) -> STM32 PB11 (RX3)
// Pi RX (GPIO15 / pin 10) -> STM32 PB10 (TX3)
// Pi GND -> STM32 GND
//
// Packet:
// V,<steerPct>,<speedPct>,<confidencePct>\n
//
// Example:
// V,35,80,92
// = look ahead RIGHT 35%, request 80% speed,
// with 92% path confidence.
HardwareSerial PiSerial(PB11, PB10);
// =====================================================
// ROBO RUMBLE - INTEGRATED AUTONOMOUS CONTROLLER
// FIXED I2C VERSION
// =====================================================
// =====================================================
// MOTOR DRIVER
// =====================================================
#define PWMA PA8
#define AIN1 PB12
#define AIN2 PB13
#define PWMB PB0
#define BIN1 PB14
#define BIN2 PB15
#define STBY PB1
// =====================================================
// EMERGENCY STOP INPUT
// =====================================================
//
// Wokwi simulation:
// PB5 uses INPUT_PULLUP.
// Button released = HIGH
// Button pressed = LOW
//
// IMPORTANT FOR THE PHYSICAL ROBOT:
// The real mushroom E-Stop must ALSO interrupt motor
// power directly. The GPIO input is only for status/
// software awareness; it is not the primary safety cut.
// =====================================================
#define ESTOP_PIN PB5
// =====================================================
// MAIN ON/OFF SWITCH INPUT
// =====================================================
//
// Wokwi simulation:
// Use a LATCHING slide/rocker switch, not a momentary button.
//
// PA15 uses INPUT_PULLUP:
// ON = switch connects PA15 to GND -> LOW
// OFF = switch open -> HIGH
//
// A broken/open signal is therefore interpreted as OFF.
//
// IMPORTANT FOR THE PHYSICAL ROBOT:
// The real accessible main ON/OFF switch should interrupt
// the robot's main electrical supply. This GPIO is only a
// Wokwi simulation/status input. The E-Stop remains separate.
// =====================================================
#define MAIN_SWITCH_PIN PA15
// =====================================================
// ENCODERS
// =====================================================
#define LEFT_ENC_A PB8
#define LEFT_ENC_B PB9
// PB10/PB11 are reserved for Raspberry Pi UART.
// Right encoder uses PA11/PA12.
#define RIGHT_ENC_A PA11
#define RIGHT_ENC_B PA12
// =====================================================
// VL53L1X FRONT COLLISION SENSORS
// =====================================================
//
// The two ToF sensors are retained because other race cars
// and track walls can become collision hazards.
//
// Shared I2C:
// SCL -> PB6
// SDA -> PB7
//
// Individual XSHUT:
// Left -> PB3
// Right -> PB4
//
// Wokwi behavioural addresses:
// Left -> 0x30
// Right -> 0x31
//
// NOTE FOR PHYSICAL HARDWARE:
// Real VL53L1X modules normally boot at the same default
// address. The final physical firmware must bring them up
// one at a time with XSHUT and assign unique addresses.
// =====================================================
#define LEFT_TOF_ADDR 0x30
#define RIGHT_TOF_ADDR 0x31
#define LEFT_XSHUT PB3
#define RIGHT_XSHUT PB4
#define SIM_RANGE_REGISTER 0x0096
// =====================================================
// MPU6050
// =====================================================
#define MPU_ADDR 0x68
#define MPU_PWR_MGMT_1 0x6B
#define MPU_GYRO_CONFIG 0x1B
#define MPU_ACCEL_XOUT_H 0x3B
#define MPU_GYRO_ZOUT_H 0x47
#define MPU_WHO_AM_I 0x75
const float GYRO_SENSITIVITY = 131.0f;
// =====================================================
// IR ARRAY
// =====================================================
const uint8_t IR_PINS[8] =
{
PA0,
PA1,
PA2,
PA3,
PA4,
PA5,
PA6,
PA7
};
// =====================================================
// ROAD-BOUNDARY INTERPRETATION
// =====================================================
//
// IMPORTANT:
// HIGH now means an IR sensor is seeing a ROAD BOUNDARY,
// not a centre line.
//
// Sensor layout:
// IR1 IR2 IR3 IR4 | IR5 IR6 IR7 IR8
// LEFT SIDE | RIGHT SIDE
//
// The weights are deliberately strongest near the
// centre of the robot. If a boundary reaches IR4 or IR5,
// the robot is close to crossing it and must turn away
// aggressively.
//
// Positive value -> steer RIGHT (boundary on the left)
// Negative value -> steer LEFT (boundary on the right)
//
// Example:
// 00000000 -> road clear, drive straight
// 10000000 -> left outer boundary, gentle right
// 00100000 -> left inner boundary, stronger right
// 00000100 -> right inner boundary, stronger left
// 00000001 -> right outer boundary, gentle left
// =====================================================
const float BOUNDARY_STEER_WEIGHTS[8] =
{
1.0f,
2.0f,
3.0f,
4.0f,
-4.0f,
-3.0f,
-2.0f,
-1.0f
};
uint8_t sensor[8];
// =====================================================
// ENVIRONMENT + SPEED PROFILES
// =====================================================
//
// WOKWI:
// RUNNING_IN_WOKWI = true
// Keeps the already-tested ~38 RPM scaled motor model.
//
// PHYSICAL ROBOT:
// RUNNING_IN_WOKWI = false
//
// Start with:
// PHYSICAL_INITIAL_TEST_PROFILE = true
// Normal target = 600 RPM ~= 1.35 m/s with 43 mm wheels.
//
// After real traction, braking, current and PID testing:
// PHYSICAL_INITIAL_TEST_PROFILE = false
// Normal straight target = 1050 RPM ~= 2.36 m/s.
//
// The faster profile is NOT permission to drive every corner
// at 2.36 m/s. Camera speed recommendation still reduces the
// target before curves, while IR/ToF/IMU safety can override it.
// =====================================================
const bool RUNNING_IN_WOKWI = true;
// Used only when RUNNING_IN_WOKWI = false.
const bool PHYSICAL_INITIAL_TEST_PROFILE = true;
// -------------------------
// WOKWI PROFILE
// -------------------------
const float WOKWI_NORMAL_BASE_RPM = 38.0f;
const float WOKWI_CAUTION_BASE_RPM = 28.0f;
const float WOKWI_SENSOR_FAULT_RPM = 20.0f;
const float WOKWI_BOUNDARY_CAUTION_RPM = 32.0f;
const float WOKWI_BOUNDARY_INNER_RPM = 28.0f;
const float WOKWI_BOUNDARY_CENTRE_RPM = 20.0f;
const float WOKWI_SKID_BASE_RPM = 22.0f;
const float WOKWI_MIN_TARGET_RPM = 8.0f;
const float WOKWI_MAX_TARGET_RPM = 40.0f;
// -------------------------
// PHYSICAL INITIAL TEST PROFILE
// -------------------------
//
// 43 mm wheels:
// 450 RPM ~= 1.01 m/s
// 600 RPM ~= 1.35 m/s
// -------------------------
const float PHYS_TEST_NORMAL_BASE_RPM = 600.0f;
const float PHYS_TEST_CAUTION_BASE_RPM = 450.0f;
const float PHYS_TEST_SENSOR_FAULT_RPM = 350.0f;
const float PHYS_TEST_BOUNDARY_CAUTION_RPM = 500.0f;
const float PHYS_TEST_BOUNDARY_INNER_RPM = 420.0f;
const float PHYS_TEST_BOUNDARY_CENTRE_RPM = 320.0f;
const float PHYS_TEST_SKID_BASE_RPM = 350.0f;
const float PHYS_TEST_MIN_TARGET_RPM = 120.0f;
const float PHYS_TEST_MAX_TARGET_RPM = 650.0f;
// -------------------------
// PHYSICAL RACE PROFILE
// -------------------------
//
// 43 mm wheels:
// 450 RPM ~= 1.01 m/s
// 525 RPM ~= 1.18 m/s
// 735 RPM ~= 1.66 m/s
// 1050 RPM ~= 2.36 m/s
//
// 1050 RPM is the first high-performance straight target.
// Speeds toward 3 m/s remain a later physical-test goal,
// not a default continuous race speed.
// -------------------------
const float PHYS_RACE_NORMAL_BASE_RPM = 1050.0f;
const float PHYS_RACE_CAUTION_BASE_RPM = 700.0f;
const float PHYS_RACE_SENSOR_FAULT_RPM = 500.0f;
const float PHYS_RACE_BOUNDARY_CAUTION_RPM = 800.0f;
const float PHYS_RACE_BOUNDARY_INNER_RPM = 650.0f;
const float PHYS_RACE_BOUNDARY_CENTRE_RPM = 500.0f;
const float PHYS_RACE_SKID_BASE_RPM = 500.0f;
const float PHYS_RACE_MIN_TARGET_RPM = 180.0f;
// Allows differential steering margin above the 1050 RPM base.
const float PHYS_RACE_MAX_TARGET_RPM = 1150.0f;
// -------------------------
// ACTIVE PROFILE
// -------------------------
const float NORMAL_BASE_RPM =
RUNNING_IN_WOKWI
? WOKWI_NORMAL_BASE_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_NORMAL_BASE_RPM
: PHYS_RACE_NORMAL_BASE_RPM
);
const float CAUTION_BASE_RPM =
RUNNING_IN_WOKWI
? WOKWI_CAUTION_BASE_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_CAUTION_BASE_RPM
: PHYS_RACE_CAUTION_BASE_RPM
);
const float SENSOR_FAULT_RPM =
RUNNING_IN_WOKWI
? WOKWI_SENSOR_FAULT_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_SENSOR_FAULT_RPM
: PHYS_RACE_SENSOR_FAULT_RPM
);
const float BOUNDARY_CAUTION_RPM =
RUNNING_IN_WOKWI
? WOKWI_BOUNDARY_CAUTION_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BOUNDARY_CAUTION_RPM
: PHYS_RACE_BOUNDARY_CAUTION_RPM
);
const float BOUNDARY_INNER_RPM =
RUNNING_IN_WOKWI
? WOKWI_BOUNDARY_INNER_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BOUNDARY_INNER_RPM
: PHYS_RACE_BOUNDARY_INNER_RPM
);
const float BOUNDARY_CENTRE_RPM =
RUNNING_IN_WOKWI
? WOKWI_BOUNDARY_CENTRE_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BOUNDARY_CENTRE_RPM
: PHYS_RACE_BOUNDARY_CENTRE_RPM
);
const float SKID_BASE_RPM =
RUNNING_IN_WOKWI
? WOKWI_SKID_BASE_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_SKID_BASE_RPM
: PHYS_RACE_SKID_BASE_RPM
);
const float MIN_TARGET_RPM =
RUNNING_IN_WOKWI
? WOKWI_MIN_TARGET_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_MIN_TARGET_RPM
: PHYS_RACE_MIN_TARGET_RPM
);
const float MAX_TARGET_RPM =
RUNNING_IN_WOKWI
? WOKWI_MAX_TARGET_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_MAX_TARGET_RPM
: PHYS_RACE_MAX_TARGET_RPM
);
// Steering gain is scaled to the active RPM range.
const float K_BOUNDARY_STEER =
RUNNING_IN_WOKWI
? 3.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 45.0f
: 75.0f
);
// =====================================================
// RACE CONTROL TIMING
// =====================================================
//
// Boundary control is deliberately much faster than the
// motor-speed PID. This lets the robot react to the road
// edge quickly while the PID handles wheel-speed control.
//
// Wokwi:
// Road/boundary loop = 10 ms (100 Hz)
// Motor PID loop = 100 ms (10 Hz)
//
// On the physical STM32, the IR/boundary loop can later
// be pushed much faster after testing.
// =====================================================
const unsigned long ROAD_CONTROL_INTERVAL_MS = 10;
unsigned long previousRoadControlTime = 0;
// Other race cars and track walls are treated as
// collision hazards, so the ToF safety layer stays active.
const bool OBSTACLE_AVOIDANCE_ENABLED = true;
// =====================================================
// WOKWI STABLE INTEGRATED SENSOR MODE
// =====================================================
//
// Earlier subsystem tests already proved the MPU6050,
// VL53L1X and INA219 logic individually. In the large
// integrated Wokwi build, several I2C devices/custom chips
// on one simulated bus can occasionally lock the simulator.
//
// This switch keeps the REAL I2C code in this same file,
// but uses deterministic internal sensor values while
// finishing the integrated racing/state-machine tests.
//
// WOKWI:
// true = use internal MPU/ToF/INA values
//
// PHYSICAL ROBOT:
// false = use the real I2C sensors on PB6/PB7
//
// This changes NO physical wiring and does NOT remove the
// sensors from the final design/documentation.
// =====================================================
// =====================================================
// WOKWI SENSOR-SOURCE SELECTION
// =====================================================
//
// We want the VL53L1X DISTANCE SLIDERS to remain interactive,
// so ToF uses the real custom Wokwi I2C chips.
//
// The MPU6050 stays internally simulated for Wokwi stability.
// The INA219 now uses the interactive Wokwi slider/custom I2C
// chip so current and voltage can be changed live during tests.
//
// PHYSICAL ROBOT:
// Set all three internal-simulation flags to false.
// =====================================================
// FALSE = read the two custom VL53L1X chips and their sliders.
const bool USE_WOKWI_INTERNAL_TOF_SIM = false;
const bool USE_WOKWI_TOF_SLIDER_CHIPS = RUNNING_IN_WOKWI;
// TRUE = use a deterministic gyro value in Wokwi.
const bool USE_WOKWI_INTERNAL_IMU_SIM = RUNNING_IN_WOKWI;
// FALSE = read the interactive INA219 Wokwi slider/custom I2C chip.
// On the physical robot this is also false, so the real INA219 is
// read over I2C at address 0x40.
const bool USE_WOKWI_INTERNAL_INA_SIM = false;
// Diagnostic/documentation flag for the current Wokwi build.
const bool USE_WOKWI_INA219_SLIDER_CHIP = RUNNING_IN_WOKWI;
// -------------------------
// INTERNAL IMU TEST VALUES
// -------------------------
const bool WOKWI_IMU_ONLINE = true;
const float WOKWI_GYRO_Z_DPS = 0.0f;
// =====================================================
// AUTONOMOUS BRIDGE HANDLING
// =====================================================
//
// Physical robot:
// Pi Camera predicts the bridge/ramp ahead.
// MPU6050 confirms chassis pitch.
// STM32 selects bridge state and safe speed automatically.
//
// No human chooses a bridge state during the race.
//
// Wokwi:
// This dedicated test automatically creates one complete
// bridge profile after MAIN SWITCH is turned ON.
// =====================================================
// Bridge control logic remains fully enabled.
// Only the automatic Wokwi bridge DEMO is disabled in this
// dedicated corner-speed test build because the bridge sequence
// has already been validated separately.
// FINAL INTEGRATED BUILD:
// Bridge logic remains active, but no scripted Wokwi bridge test runs.
const bool WOKWI_AUTO_BRIDGE_TEST = false;
const float WOKWI_MANUAL_PITCH_DEG = 0.0f;
const int WOKWI_MANUAL_BRIDGE_CONFIDENCE_PCT = 0;
// Automatic Wokwi timeline, measured from first ON state.
const unsigned long BRIDGE_TEST_FLAT_MS = 3000;
const unsigned long BRIDGE_TEST_APPROACH_MS = 5000;
const unsigned long BRIDGE_TEST_CLIMB_MS = 8000;
const unsigned long BRIDGE_TEST_CREST_MS = 9500;
const unsigned long BRIDGE_TEST_DESCENT_MS = 12500;
const unsigned long BRIDGE_TEST_EXIT_MS = 14500;
// First-pass thresholds for a mild bridge slope.
// Retune these on the physical bridge after measuring
// real pitch noise and the actual bridge angle.
const int BRIDGE_CAMERA_ENTER_CONFIDENCE_PCT = 70;
const int BRIDGE_CAMERA_CLEAR_CONFIDENCE_PCT = 40;
const float BRIDGE_CLIMB_ENTER_PITCH_DEG = 2.0f;
const float BRIDGE_CREST_PITCH_DEG = 1.2f;
const float BRIDGE_DESCENT_ENTER_PITCH_DEG = -2.0f;
const float BRIDGE_FLAT_PITCH_DEG = 1.0f;
const unsigned long BRIDGE_EXIT_CONFIRM_MS = 800;
// =====================================================
// BRIDGE SPEED PROFILE
// =====================================================
//
// Wokwi keeps the mild-slope values already tested.
//
// Physical initial:
// normal 600
// approach 540
// climb 510
// crest 480
// descent 480
// exit 540
//
// Physical race:
// normal 1050
// approach 900
// climb 850
// crest 800
// descent 800
// exit 900
//
// Final physical values must be tuned using the real bridge.
// =====================================================
const float WOKWI_BRIDGE_APPROACH_RPM = 34.0f;
const float WOKWI_BRIDGE_CLIMB_RPM = 32.0f;
const float WOKWI_BRIDGE_CREST_RPM = 30.0f;
const float WOKWI_BRIDGE_DESCENT_RPM = 30.0f;
const float WOKWI_BRIDGE_EXIT_RPM = 34.0f;
const float PHYS_TEST_BRIDGE_APPROACH_RPM = 540.0f;
const float PHYS_TEST_BRIDGE_CLIMB_RPM = 510.0f;
const float PHYS_TEST_BRIDGE_CREST_RPM = 480.0f;
const float PHYS_TEST_BRIDGE_DESCENT_RPM = 480.0f;
const float PHYS_TEST_BRIDGE_EXIT_RPM = 540.0f;
const float PHYS_RACE_BRIDGE_APPROACH_RPM = 900.0f;
const float PHYS_RACE_BRIDGE_CLIMB_RPM = 850.0f;
const float PHYS_RACE_BRIDGE_CREST_RPM = 800.0f;
const float PHYS_RACE_BRIDGE_DESCENT_RPM = 800.0f;
const float PHYS_RACE_BRIDGE_EXIT_RPM = 900.0f;
const float BRIDGE_APPROACH_RPM =
RUNNING_IN_WOKWI
? WOKWI_BRIDGE_APPROACH_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BRIDGE_APPROACH_RPM
: PHYS_RACE_BRIDGE_APPROACH_RPM
);
const float BRIDGE_CLIMB_RPM =
RUNNING_IN_WOKWI
? WOKWI_BRIDGE_CLIMB_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BRIDGE_CLIMB_RPM
: PHYS_RACE_BRIDGE_CLIMB_RPM
);
const float BRIDGE_CREST_RPM =
RUNNING_IN_WOKWI
? WOKWI_BRIDGE_CREST_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BRIDGE_CREST_RPM
: PHYS_RACE_BRIDGE_CREST_RPM
);
const float BRIDGE_DESCENT_RPM =
RUNNING_IN_WOKWI
? WOKWI_BRIDGE_DESCENT_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BRIDGE_DESCENT_RPM
: PHYS_RACE_BRIDGE_DESCENT_RPM
);
const float BRIDGE_EXIT_RPM =
RUNNING_IN_WOKWI
? WOKWI_BRIDGE_EXIT_RPM
: (
PHYSICAL_INITIAL_TEST_PROFILE
? PHYS_TEST_BRIDGE_EXIT_RPM
: PHYS_RACE_BRIDGE_EXIT_RPM
);
const float BRIDGE_APPROACH_STEER_RPM =
RUNNING_IN_WOKWI
? 8.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 120.0f
: 200.0f
);
const float BRIDGE_CLIMB_STEER_RPM =
RUNNING_IN_WOKWI
? 7.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 100.0f
: 170.0f
);
const float BRIDGE_CREST_STEER_RPM =
RUNNING_IN_WOKWI
? 6.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 90.0f
: 150.0f
);
const float BRIDGE_DESCENT_STEER_RPM =
RUNNING_IN_WOKWI
? 6.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 90.0f
: 150.0f
);
const float BRIDGE_EXIT_STEER_RPM =
RUNNING_IN_WOKWI
? 8.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 120.0f
: 200.0f
);
float wokwiSimulatedPitchDeg = 0.0f;
int wokwiSimulatedBridgeConfidencePct = 0;
bool wokwiBridgeTestStarted = false;
unsigned long wokwiBridgeTestStartTime = 0;
// -------------------------
// INTERNAL INA219 FALLBACK VALUES
// -------------------------
//
// These values are used only if USE_WOKWI_INTERNAL_INA_SIM
// is changed back to true. In the current Wokwi build, the
// sliders control voltage and current live over I2C.
const bool WOKWI_INA_ONLINE = true;
const float WOKWI_BUS_VOLTAGE_V = 5.0f;
const float WOKWI_MOTOR_CURRENT_MA = 500.0f;
// =====================================================
// RASPBERRY PI CAMERA / PREDICTIVE STEERING
// =====================================================
//
// Camera/Pi:
// sees the road ahead, predicts the next bend,
// and recommends steering + approach speed.
//
// IR array:
// reacts to the boundary directly under the car.
//
// STM32:
// remains the final authority for motors, PID,
// skid protection, stall protection and E-Stop.
//
// Packet from Pi:
// V,<steerPct>,<speedPct>,<confidencePct>,<bridgeConfidencePct>
//
// steerPct:
// -100 = hard LEFT
// 0 = straight
// +100 = hard RIGHT
//
// speedPct:
// 50..100 recommended race speed
//
// confidencePct:
// 0..100 path-detection confidence
// =====================================================
const unsigned long VISION_TIMEOUT_MS = 250;
// =====================================================
// WOKWI INTERNAL CAMERA / PI SIMULATOR
// =====================================================
//
// Wokwi cannot reproduce the real Raspberry Pi Camera
// processing pipeline. To keep testing reliable, this mode
// injects the SAME steering/speed/confidence command that the
// physical Raspberry Pi will later send over UART.
//
// Leave this TRUE while finishing the Wokwi controller.
// Set it FALSE on the physical robot so PB11 receives the
// real Raspberry Pi UART packets.
//
// FINAL INTEGRATED WOKWI VISION PLACEHOLDER:
//
// No scripted test is run automatically.
//
// The internal vision source starts neutral:
// steering = 0%
// speed = 100%
// confidence = 95%
// bridge confidence = 0%
// corner severity = 0%
//
// Therefore, after the operator deliberately starts the race,
// the default simulated road is simply a clear STRAIGHT.
// The normal IR, ToF, IMU, INA219, encoder, PID, bridge,
// corner-speed, stall, skid, E-Stop and fail-safe logic remains
// fully integrated and will react only when its inputs require it.
// =====================================================
const bool USE_WOKWI_INTERNAL_VISION_SIM = RUNNING_IN_WOKWI;
// FINAL INTEGRATED BUILD:
// No scripted/automatic corner test is allowed to run.
const bool WOKWI_AUTO_CORNER_SPEED_TEST = false;
const int WOKWI_VISION_STEER_PCT = 0;
const int WOKWI_VISION_SPEED_PCT = 100;
const int WOKWI_VISION_CONFIDENCE_PCT = 95;
const int WOKWI_VISION_CORNER_SEVERITY_PCT = 0;
const bool WOKWI_VISION_ONLINE = true;
// Automatic corner-test timing.
//
// This test now has its OWN timer and does not depend on the
// bridge test at all.
//
// 0-3 s : STRAIGHT
// 3-6 s : GENTLE RIGHT
// 6-9 s : MEDIUM LEFT
// 9-12 s : SHARP RIGHT
// 12-15 s : VERY SHARP LEFT
// >15 s : STRAIGHT AGAIN
const unsigned long CORNER_TEST_GENTLE_MS = 3000;
const unsigned long CORNER_TEST_MEDIUM_MS = 6000;
const unsigned long CORNER_TEST_SHARP_MS = 9000;
const unsigned long CORNER_TEST_VERY_SHARP_MS = 12000;
const unsigned long CORNER_TEST_END_MS = 15000;
bool wokwiCornerTestStarted = false;
unsigned long wokwiCornerTestStartTime = 0;
const unsigned long WOKWI_VISION_INTERVAL_MS = 40;
unsigned long previousWokwiVisionTime = 0;
const int VISION_MIN_CONFIDENCE_PCT = 45;
const int VISION_MIN_SPEED_PCT = 50;
const int VISION_MAX_SPEED_PCT = 100;
const float VISION_MAX_STEER_RPM =
RUNNING_IN_WOKWI
? 10.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 150.0f
: 250.0f
);
// Minimum speed the camera is allowed to request for a
// predicted sharp corner.
//
// Physical lower limit ~450 RPM ~= 1.0 m/s with 43 mm wheels.
const float VISION_MIN_BASE_RPM =
RUNNING_IN_WOKWI
? 20.0f
: 450.0f;
// Speed used if the camera is offline/stale/low-confidence.
const float VISION_FALLBACK_RPM =
RUNNING_IN_WOKWI
? 28.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 450.0f
: 550.0f
);
// =====================================================
// DYNAMIC CORNER-SPEED CONTROL
// =====================================================
//
// Cornering force:
//
// F = m * v^2 / R
//
// Because speed is squared, the car must slow down
// non-linearly as the upcoming road curvature increases.
//
// The Pi will estimate CORNER SEVERITY from upcoming road
// curvature. STM32 then applies an independent speed cap:
//
// speedFactor = 1 / sqrt(1 + K * severity)
//
// where severity is 0.0..1.0.
//
// With K = 2:
// severity 0% -> 100% straight speed
// severity 25% -> ~82%
// severity 50% -> ~71%
// severity 75% -> ~63%
// severity 100% -> ~58%
//
// In physical race mode with 1050 RPM straight speed this is
// roughly:
// 0% -> 1050 RPM ~= 2.36 m/s
// 25% -> 857 RPM ~= 1.93 m/s
// 50% -> 742 RPM ~= 1.67 m/s
// 75% -> 664 RPM ~= 1.50 m/s
// 100% -> 606 RPM ~= 1.36 m/s
//
// The Pi's own speed recommendation can request an EVEN LOWER
// speed. STM32 always chooses the safer/lower of the two.
// =====================================================
const float CORNER_CURVATURE_GAIN = 2.0f;
int visionSteerPct = 0;
int visionSpeedPct = 100;
int visionConfidencePct = 0;
// 0..100 confidence that the camera sees a bridge/ramp ahead.
int visionBridgeConfidencePct = 0;
// 0 = straight road ahead, 100 = strongest expected corner.
// This is look-ahead geometry, not merely current steering angle.
int visionCornerSeverityPct = 0;
bool visionPacketValid = false;
unsigned long lastVisionPacketTime = 0;
char visionRxBuffer[48];
uint8_t visionRxIndex = 0;
// Pi UART diagnostics - visible in the NORMAL Serial Monitor.
unsigned long piRxByteCount = 0;
unsigned long piGoodPacketCount = 0;
unsigned long piBadPacketCount = 0;
char piLastRawPacket[48] = "(none)";
// =====================================================
// TOF / TTC
// =====================================================
// 20 Hz ToF/TTC update.
// A 500 ms interval is too slow for a racing robot.
const unsigned long TOF_INTERVAL_MS = 50;
const float MIN_CLOSING_SPEED_MM_S = 50.0f;
const float TTC_CRITICAL_S = 0.50f;
const float TTC_AVOID_S = 1.00f;
const float TTC_CAUTION_S = 2.00f;
const uint16_t CRITICAL_RANGE_MM = 180;
const uint16_t AVOID_RANGE_MM = 400;
const uint16_t CAUTION_RANGE_MM = 800;
const uint16_t SIDE_TOLERANCE_MM = 100;
// =====================================================
// INA219 CURRENT / POWER SENSOR
// =====================================================
//
// Wokwi behavioural model:
// Address 0x40
// Register 0x02 = bus voltage
// Register 0x04 = current
//
// IMPORTANT:
// These current thresholds are SIMULATION STARTING VALUES.
// Tune them against the real motor stall-current measurements
// before using them on the physical robot.
// =====================================================
#define INA219_ADDR 0x40
#define INA219_REG_BUS_VOLTAGE 0x02
#define INA219_REG_CURRENT 0x04
const unsigned long INA_INTERVAL_MS = 100;
// Wokwi keeps the already-tested slider thresholds.
//
// Physical starting values are intentionally conservative and
// must be calibrated on the assembled car. The encoder-speed
// condition + 1 s confirmation + manual reset latch remain active.
const float HIGH_CURRENT_MA =
RUNNING_IN_WOKWI
? 1200.0f
: 500.0f;
const float STALL_CURRENT_MA =
RUNNING_IN_WOKWI
? 1800.0f
: 650.0f;
// Reference only. Confirmed stall uses MAIN OFF -> ON reset.
const float STALL_RESET_CURRENT_MA =
RUNNING_IN_WOKWI
? 800.0f
: 400.0f;
const float STALL_RPM_THRESHOLD =
RUNNING_IN_WOKWI
? 5.0f
: 80.0f;
const float STALL_COMMAND_RPM_THRESHOLD =
RUNNING_IN_WOKWI
? 10.0f
: 200.0f;
const unsigned long STALL_CONFIRM_MS = 1000;
// =====================================================
// ENCODERS
// =====================================================
const float CPR = 280.0f;
// RPM sanity range.
// Wokwi: 42.86 RPM model -> 60 RPM sanity ceiling.
// Physical: 1500 RPM no-load motor -> 1800 RPM diagnostic ceiling.
const float MAX_VALID_RPM =
RUNNING_IN_WOKWI
? 60.0f
: 1800.0f;
const int8_t quadratureTable[16] =
{
0, +1, -1, 0,
-1, 0, 0, +1,
+1, 0, 0, -1,
0, -1, +1, 0
};
// =====================================================
// PID
// =====================================================
// Wokwi gains remain exactly as tested.
// Physical gains are safe STARTING VALUES only and require
// real encoder/PWM tuning before competition-speed operation.
float Kp =
RUNNING_IN_WOKWI
? 2.0f
: 0.25f;
float Ki =
RUNNING_IN_WOKWI
? 0.60f
: 0.08f;
float Kd =
RUNNING_IN_WOKWI
? 0.05f
: 0.01f;
// 20 Hz wheel-speed PID for the high-speed Wokwi build.
// IR road-edge control remains faster at 100 Hz.
const unsigned long PID_INTERVAL_MS = 50;
// =====================================================
// PID STABILITY / OVERSPEED CONTROL
// =====================================================
//
// Wokwi N20 model calibration:
// 255 PWM -> about 42.86 RPM
//
// We therefore use a speed-to-PWM feed-forward term.
// PID only trims the remaining error.
//
// Physical robot:
// Measure the real PWM-vs-RPM relationship and retune.
// =====================================================
const float WOKWI_MOTOR_MAX_RPM = 42.86f;
const float PHYSICAL_MOTOR_NO_LOAD_RPM = 1500.0f;
// Physical feed-forward is only a first approximation.
// Replace it with measured PWM-vs-loaded-RPM data.
const float PWM_FEEDFORWARD_PER_RPM =
RUNNING_IN_WOKWI
? (
255.0f /
WOKWI_MOTOR_MAX_RPM
)
: (
255.0f /
PHYSICAL_MOTOR_NO_LOAD_RPM
);
// Integral anti-windup bound.
const float PID_INTEGRAL_LIMIT =
RUNNING_IN_WOKWI
? 15.0f
: 400.0f;
// Smooth encoder RPM samples before PID.
const float RPM_FILTER_ALPHA = 0.35f;
// Bleed positive integral if actual RPM exceeds setpoint.
const float OVERSPEED_MARGIN_RPM =
RUNNING_IN_WOKWI
? 1.0f
: 25.0f;
const float OVERSPEED_INTEGRAL_BLEED = 0.50f;
// =====================================================
// SPEED RAMP / SLEW-RATE LIMITER
// =====================================================
//
// Navigation can request a new wheel speed immediately,
// but the PID follows a ramped setpoint instead of jumping
// straight to the requested value.
//
// This reduces launch wheel-slip, current spikes, false
// stall detection, and violent weight transfer.
//
// Wokwi starting values:
// acceleration = 30 RPM/s
// deceleration = 120 RPM/s
//
// At the 50 ms PID interval this is about:
// +1.5 RPM per PID step while accelerating
// -6.0 RPM per PID step while decelerating
//
// Deceleration is deliberately faster so the robot can
// react quickly to corners and boundaries.
//
// IMPORTANT: these are simulation values. The physical
// 1500 RPM N20 robot must be retuned using measured grip,
// current draw, mass, bridge slope and wheel-slip.
// =====================================================
const float ACCEL_RAMP_RPM_PER_SEC =
RUNNING_IN_WOKWI
? 30.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 300.0f
: 450.0f
);
const float DECEL_RAMP_RPM_PER_SEC =
RUNNING_IN_WOKWI
? 120.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 450.0f
: 700.0f
);
// =====================================================
// SOFTWARE PWM
// =====================================================
const unsigned long PWM_PERIOD_US = 10000;
// =====================================================
// AVOIDANCE SPEED PROFILE
// =====================================================
const float AVOID_SLOW_RPM =
RUNNING_IN_WOKWI
? 14.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 220.0f
: 400.0f
);
const float AVOID_FAST_RPM =
RUNNING_IN_WOKWI
? 34.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 540.0f
: 900.0f
);
const float CRITICAL_AVOID_SLOW_RPM =
RUNNING_IN_WOKWI
? 8.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 120.0f
: 180.0f
);
const float CRITICAL_AVOID_FAST_RPM =
RUNNING_IN_WOKWI
? 38.0f
: (
PHYSICAL_INITIAL_TEST_PROFILE
? 600.0f
: 1050.0f
);
// =====================================================
// NAVIGATION STATES
// =====================================================
enum NavigationState
{
STATE_EMERGENCY_STOP,
STATE_VISION_DRIVE,
STATE_VISION_FALLBACK,
STATE_ROAD_DRIVE,
STATE_BOUNDARY_CORRECT_LEFT,
STATE_BOUNDARY_CORRECT_RIGHT,
STATE_BOUNDARY_CAUTION,
STATE_BOUNDARY_ESCAPE,
STATE_CAUTION,
STATE_AVOID_LEFT,
STATE_AVOID_RIGHT,
STATE_CRITICAL_AVOID_LEFT,
STATE_CRITICAL_AVOID_RIGHT,
STATE_CRITICAL_STOP,
// Obstacle requires an avoidance turn, but the road
// boundary blocks that escape direction.
STATE_NO_SAFE_PATH,
STATE_MOTOR_STALL,
STATE_SKID_CAUTION,
STATE_SENSOR_FAULT,
// Appended so all previously validated state numbers stay unchanged.
STATE_SYSTEM_OFF,
STATE_BRIDGE_APPROACH,
STATE_BRIDGE_CLIMB,
STATE_BRIDGE_CREST,
STATE_BRIDGE_DESCENT,
STATE_BRIDGE_EXIT
};
// Safe boot: the race controller always starts OFF.
// It cannot drive until the MAIN switch has been observed OFF
// and is then deliberately switched ON.
NavigationState currentState = STATE_SYSTEM_OFF;
NavigationState previousState = STATE_SYSTEM_OFF;
// Counts impossible/invalid state values detected at runtime.
// Any invalid state immediately falls back to CRITICAL STOP.
unsigned long invalidStateRecoveryCount = 0;
bool emergencyStopActive = false;
// =====================================================
// DEBOUNCED MAIN SWITCH + RACE-START INTERLOCK
// =====================================================
//
// mainSwitchRawOn:
// immediate electrical reading from PA15.
//
// mainSwitchOn:
// stable/debounced switch state used by the controller.
//
// The robot ALWAYS boots with the race interlock locked.
// It must observe a stable MAIN OFF state, and only a later
// stable OFF -> ON transition is allowed to start the race.
//
// This also prevents one physical switch movement from being
// interpreted as several starts because of contact bounce.
// =====================================================
bool mainSwitchRawOn = false;
bool mainSwitchOn = false;
bool mainSwitchSeenOffSinceBoot = false;
bool raceStartInterlockCleared = false;
unsigned long mainSwitchLastRawChangeMs = 0;
// 60 ms is comfortably above normal mechanical switch bounce
// and is negligible compared with race setup time.
const unsigned long MAIN_SWITCH_DEBOUNCE_MS = 60;
// =====================================================
// ROAD / BOUNDARY DATA
// =====================================================
//
// boundarySteerError:
// positive -> steer RIGHT
// negative -> steer LEFT
//
// leftBoundarySeverity/rightBoundarySeverity:
// 0 = no boundary
// 1 = outer sensor
// 2 = next sensor
// 3 = inner sensor
// 4 = centre-adjacent sensor
// =====================================================
float boundarySteerError = 0.0f;
float lastBoundarySteerError = 0.0f;
float leftBoundarySeverity = 0.0f;
float rightBoundarySeverity = 0.0f;
float boundarySeverity = 0.0f;
bool boundaryDetected = false;
bool centreBoundaryDetected = false;
bool bothSidesBoundaryDetected = false;
int activeBoundarySensors = 0;
// =====================================================
// TOF DATA
// =====================================================
uint16_t leftDistance = 2000;
uint16_t rightDistance = 2000;
uint16_t previousLeftDistance = 2000;
uint16_t previousRightDistance = 2000;
float leftClosingSpeed = 0.0f;
float rightClosingSpeed = 0.0f;
float leftTTC = -1.0f;
float rightTTC = -1.0f;
bool tofValid = false;
bool previousToFValid = false;
unsigned long previousToFTime = 0;
// =====================================================
// INA219 DATA
// =====================================================
float busVoltageV = 0.0f;
float motorCurrentMA = 0.0f;
bool inaValid = false;
bool motorStallDetected = false;
bool motorStallLatched = false;
// Bit 0 = left wheel, bit 1 = right wheel.
// These let us identify a one-wheel stall instead of averaging
// both wheel speeds together.
uint8_t stallCandidateMask = 0;
uint8_t stallLatchedSideMask = 0;
unsigned long previousINATime = 0;
unsigned long stallConditionStart = 0;
// =====================================================
// IMU
// =====================================================
float gyroZ = 0.0f;
// 0 = flat, positive = climbing, negative = descending.
float pitchDeg = 0.0f;
bool imuValid = false;
bool possibleSkid = false;
enum BridgePhase
{
BRIDGE_NONE,
BRIDGE_APPROACH,
BRIDGE_CLIMB,
BRIDGE_CREST,
BRIDGE_DESCENT,
BRIDGE_EXIT
};
BridgePhase bridgePhase = BRIDGE_NONE;
float maximumBridgeClimbPitchDeg = 0.0f;
unsigned long bridgeExitStartTime = 0;
// =====================================================
// ENCODER STATE
// =====================================================
long leftEncoderCount = 0;
long rightEncoderCount = 0;
long previousLeftPIDCount = 0;
long previousRightPIDCount = 0;
uint8_t previousLeftEncoderState = 0;
uint8_t previousRightEncoderState = 0;
// =====================================================
// RPM
// =====================================================
// Filtered encoder RPM used by the controller.
float leftRPM = 0.0f;
float rightRPM = 0.0f;
// Latest raw encoder RPM samples.
float leftRawRPM = 0.0f;
float rightRawRPM = 0.0f;
bool leftRPMFilterInitialised = false;
bool rightRPMFilterInitialised = false;
// True when the most recent encoder RPM sample was finite
// and inside the physically reasonable simulation range.
bool leftRPMValid = true;
bool rightRPMValid = true;
// Navigation-requested wheel speeds.
float leftTargetRPM = NORMAL_BASE_RPM;
float rightTargetRPM = NORMAL_BASE_RPM;
// PID setpoints after acceleration/deceleration limiting.
// Start at zero for a controlled soft launch.
float leftRampedTargetRPM = 0.0f;
float rightRampedTargetRPM = 0.0f;
// =====================================================
// PID STATE
// =====================================================
float leftIntegral = 0.0f;
float rightIntegral = 0.0f;
float leftPreviousError = 0.0f;
float rightPreviousError = 0.0f;
unsigned long previousPIDTime = 0;
// =====================================================
// PWM STATE
// =====================================================
// Start with zero motor drive. PID increases PWM only
// as the ramped target rises.
int leftPWM = 0;
int rightPWM = 0;
bool leftPWMState = false;
bool rightPWMState = false;
unsigned long pwmPeriodStart = 0;
// =====================================================
// SERIAL TIMING
// =====================================================
const unsigned long PRINT_INTERVAL_MS = 1500;
unsigned long previousPrintTime = 0;
// =====================================================
// ENCODERS
// =====================================================
void readLeftEncoder()
{
uint8_t a = digitalRead(LEFT_ENC_A);
uint8_t b = digitalRead(LEFT_ENC_B);
uint8_t current = (a << 1) | b;
if (current == previousLeftEncoderState)
return;
uint8_t index =
(previousLeftEncoderState << 2) | current;
leftEncoderCount += quadratureTable[index];
previousLeftEncoderState = current;
}
void readRightEncoder()
{
uint8_t a = digitalRead(RIGHT_ENC_A);
uint8_t b = digitalRead(RIGHT_ENC_B);
uint8_t current = (a << 1) | b;
if (current == previousRightEncoderState)
return;
uint8_t index =
(previousRightEncoderState << 2) | current;
rightEncoderCount += quadratureTable[index];
previousRightEncoderState = current;
}
// =====================================================
// SOFTWARE PWM
// =====================================================
void updateSoftwarePWM()
{
unsigned long now = micros();
if (
now - pwmPeriodStart >=
PWM_PERIOD_US
)
{
pwmPeriodStart = now;
}
unsigned long position =
now - pwmPeriodStart;
// LEFT
unsigned long leftHighTime =
((unsigned long)leftPWM * PWM_PERIOD_US)
/ 255UL;
bool desiredLeft;
if (leftPWM <= 0)
desiredLeft = false;
else if (leftPWM >= 255)
desiredLeft = true;
else
desiredLeft = position < leftHighTime;
if (desiredLeft != leftPWMState)
{
leftPWMState = desiredLeft;
digitalWrite(
PWMA,
leftPWMState ? HIGH : LOW
);
}
// RIGHT
unsigned long rightHighTime =
((unsigned long)rightPWM * PWM_PERIOD_US)
/ 255UL;
bool desiredRight;
if (rightPWM <= 0)
desiredRight = false;
else if (rightPWM >= 255)
desiredRight = true;
else
desiredRight = position < rightHighTime;
if (desiredRight != rightPWMState)
{
rightPWMState = desiredRight;
digitalWrite(
PWMB,
rightPWMState ? HIGH : LOW
);
}
}
void serviceMotors()
{
updateSoftwarePWM();
readLeftEncoder();
readRightEncoder();
}
// =====================================================
// MOTOR DIRECTION
// =====================================================
void motorsForward()
{
// Direction commands are prepared here. The motor
// driver is enabled only while the E-Stop is released.
digitalWrite(
STBY,
(
emergencyStopActive
||
!mainSwitchOn
||
!raceStartInterlockCleared
)
? LOW
: HIGH
);
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
// =====================================================
// EMERGENCY STOP
// =====================================================
void readEmergencyStop()
{
emergencyStopActive =
(
digitalRead(ESTOP_PIN)
==
LOW
);
}
void applyEmergencyStopImmediately()
{
// Do not wait for the 250 ms PID update.
currentState =
STATE_EMERGENCY_STOP;
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
leftPWM = 0;
rightPWM = 0;
leftPWMState = false;
rightPWMState = false;
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
// Hardware-driver shutdown in the Wokwi controller.
digitalWrite(STBY, LOW);
}
// =====================================================
// MAIN ON/OFF SWITCH
// =====================================================
void resetDriveMemoryForRaceStart()
{
// Start both motor channels from exactly the same neutral state.
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
leftIntegral = 0.0f;
rightIntegral = 0.0f;
leftPreviousError = 0.0f;
rightPreviousError = 0.0f;
leftPWM = 0;
rightPWM = 0;
leftPWMState = false;
rightPWMState = false;
// Neutral navigation memory.
boundarySteerError = 0.0f;
lastBoundarySteerError = 0.0f;
// Neutral internal vision placeholder.
visionSteerPct = 0;
visionSpeedPct = 100;
visionConfidencePct = 95;
visionBridgeConfidencePct = 0;
visionCornerSeverityPct = 0;
// No scripted test is allowed to carry into the race.
bridgePhase = BRIDGE_NONE;
wokwiBridgeTestStarted = false;
wokwiCornerTestStarted = false;
// PID timing starts fresh so the first ramp step is symmetrical.
previousPIDTime = millis();
// Motor driver remains disabled until the main loop reaches
// the normal enabled section.
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(STBY, LOW);
}
void initialiseMainSwitch()
{
// INPUT_PULLUP:
// LOW = ON
// HIGH = OFF
mainSwitchRawOn =
(
digitalRead(MAIN_SWITCH_PIN)
==
LOW
);
mainSwitchOn =
mainSwitchRawOn;
mainSwitchLastRawChangeMs =
millis();
// IMPORTANT:
// Never start a race merely because the switch happened to be
// ON when the MCU/Wokwi simulation booted.
raceStartInterlockCleared = false;
if (!mainSwitchOn)
{
mainSwitchSeenOffSinceBoot = true;
}
else
{
mainSwitchSeenOffSinceBoot = false;
}
// Safe startup outputs.
applySystemOffImmediately();
}
void readMainSwitch()
{
bool rawOn =
(
digitalRead(MAIN_SWITCH_PIN)
==
LOW
);
// A raw transition only starts/restarts the debounce timer.
// It does NOT immediately change the controller state.
if (rawOn != mainSwitchRawOn)
{
mainSwitchRawOn = rawOn;
mainSwitchLastRawChangeMs = millis();
}
// Wait until the raw state has remained unchanged long enough.
if (
(millis() - mainSwitchLastRawChangeMs)
<
MAIN_SWITCH_DEBOUNCE_MS
)
{
return;
}
// No new stable switch transition.
if (mainSwitchRawOn == mainSwitchOn)
{
return;
}
// Accept exactly one stable transition.
bool oldStableOn = mainSwitchOn;
mainSwitchOn = mainSwitchRawOn;
// ===================================================
// STABLE ON -> OFF
// ===================================================
if (
oldStableOn
&&
!mainSwitchOn
)
{
// The robot is now deliberately OFF.
raceStartInterlockCleared = false;
mainSwitchSeenOffSinceBoot = true;
// A confirmed motor stall may only be cleared by this
// deliberate MAIN OFF action.
resetMotorStallLatchWhileSystemOff();
// Immediately make both motor channels identical and safe.
applySystemOffImmediately();
DebugSerial.println(
"[POWER] MAIN SWITCH -> OFF : SYSTEM SAFE"
);
return;
}
// ===================================================
// FIRST/STABLE OFF OBSERVATION
// ===================================================
if (!mainSwitchOn)
{
mainSwitchSeenOffSinceBoot = true;
raceStartInterlockCleared = false;
applySystemOffImmediately();
return;
}
// ===================================================
// STABLE OFF -> ON = ONE RACE START
// ===================================================
if (
!oldStableOn
&&
mainSwitchOn
&&
mainSwitchSeenOffSinceBoot
&&
!raceStartInterlockCleared
)
{
resetDriveMemoryForRaceStart();
raceStartInterlockCleared = true;
DebugSerial.println(
"[START] MAIN OFF -> ON : RACE START ENABLED"
);
return;
}
// Any other situation remains locked for safety.
raceStartInterlockCleared = false;
}
void applySystemOffImmediately()
{
currentState =
STATE_SYSTEM_OFF;
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
leftPWM = 0;
rightPWM = 0;
leftPWMState = false;
rightPWMState = false;
leftIntegral = 0.0f;
rightIntegral = 0.0f;
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(STBY, LOW);
}
// =====================================================
// WOKWI AUTONOMOUS BRIDGE SCENARIO
// =====================================================
void updateWokwiBridgeScenario()
{
if (!USE_WOKWI_INTERNAL_IMU_SIM)
{
return;
}
if (!WOKWI_AUTO_BRIDGE_TEST)
{
wokwiSimulatedPitchDeg =
WOKWI_MANUAL_PITCH_DEG;
wokwiSimulatedBridgeConfidencePct =
WOKWI_MANUAL_BRIDGE_CONFIDENCE_PCT;
return;
}
if (!wokwiBridgeTestStarted)
{
wokwiBridgeTestStarted = true;
wokwiBridgeTestStartTime = millis();
}
unsigned long elapsed =
millis() -
wokwiBridgeTestStartTime;
if (elapsed < BRIDGE_TEST_FLAT_MS)
{
wokwiSimulatedPitchDeg = 0.0f;
wokwiSimulatedBridgeConfidencePct = 0;
return;
}
if (elapsed < BRIDGE_TEST_APPROACH_MS)
{
wokwiSimulatedPitchDeg = 0.0f;
wokwiSimulatedBridgeConfidencePct = 90;
return;
}
if (elapsed < BRIDGE_TEST_CLIMB_MS)
{
wokwiSimulatedPitchDeg = 4.5f;
wokwiSimulatedBridgeConfidencePct = 95;
return;
}
if (elapsed < BRIDGE_TEST_CREST_MS)
{
wokwiSimulatedPitchDeg = 1.0f;
wokwiSimulatedBridgeConfidencePct = 95;
return;
}
if (elapsed < BRIDGE_TEST_DESCENT_MS)
{
wokwiSimulatedPitchDeg = -4.5f;
wokwiSimulatedBridgeConfidencePct = 90;
return;
}
if (elapsed < BRIDGE_TEST_EXIT_MS)
{
wokwiSimulatedPitchDeg = 0.0f;
wokwiSimulatedBridgeConfidencePct = 20;
return;
}
wokwiSimulatedPitchDeg = 0.0f;
wokwiSimulatedBridgeConfidencePct = 0;
}
// =====================================================
// RASPBERRY PI VISION UART
// =====================================================
bool visionIsOnline()
{
if (!visionPacketValid)
return false;
return
(
millis() -
lastVisionPacketTime
)
<=
VISION_TIMEOUT_MS;
}
bool visionIsTrusted()
{
return
visionIsOnline()
&&
visionConfidencePct >=
VISION_MIN_CONFIDENCE_PCT;
}
const char *cornerClassName()
{
if (visionCornerSeverityPct < 15)
{
return "STRAIGHT";
}
if (visionCornerSeverityPct < 35)
{
return "GENTLE";
}
if (visionCornerSeverityPct < 60)
{
return "MEDIUM";
}
if (visionCornerSeverityPct < 85)
{
return "SHARP";
}
return "VERY SHARP";
}
float getVisionCornerSpeedCapRPM()
{
if (!visionIsTrusted())
{
return VISION_FALLBACK_RPM;
}
float severity =
constrain(
(float)visionCornerSeverityPct /
100.0f,
0.0f,
1.0f
);
// F = m*v^2/R means the speed limit should fall
// non-linearly as curvature demand rises.
float speedFactor =
1.0f /
sqrtf(
1.0f
+
CORNER_CURVATURE_GAIN *
severity
);
float capRPM =
NORMAL_BASE_RPM *
speedFactor;
return
constrain(
capRPM,
VISION_MIN_BASE_RPM,
NORMAL_BASE_RPM
);
}
float getVisionBaseRPM()
{
if (!visionIsTrusted())
{
return VISION_FALLBACK_RPM;
}
int safeSpeedPct =
constrain(
visionSpeedPct,
VISION_MIN_SPEED_PCT,
VISION_MAX_SPEED_PCT
);
float cameraRequestedRPM =
NORMAL_BASE_RPM *
(
(float)safeSpeedPct /
100.0f
);
float cornerSpeedCapRPM =
getVisionCornerSpeedCapRPM();
float safeRPM =
min(
cameraRequestedRPM,
cornerSpeedCapRPM
);
return
constrain(
safeRPM,
VISION_MIN_BASE_RPM,
NORMAL_BASE_RPM
);
}
bool parseVisionPacket(
char *packet
)
{
int steer = 0;
int speed = 0;
int confidence = 0;
int bridgeConfidence = 0;
int cornerSeverity = 0;
// Backward compatible packet formats:
// V,steer,speed,confidence
// V,steer,speed,confidence,bridgeConfidence
// V,steer,speed,confidence,bridgeConfidence,cornerSeverity
int parsed =
sscanf(
packet,
"V,%d,%d,%d,%d,%d",
&steer,
&speed,
&confidence,
&bridgeConfidence,
&cornerSeverity
);
if (
parsed != 3
&&
parsed != 4
&&
parsed != 5
)
{
return false;
}
visionSteerPct =
constrain(
steer,
-100,
100
);
visionSpeedPct =
constrain(
speed,
VISION_MIN_SPEED_PCT,
VISION_MAX_SPEED_PCT
);
visionConfidencePct =
constrain(
confidence,
0,
100
);
visionBridgeConfidencePct =
constrain(
parsed >= 4
? bridgeConfidence
: 0,
0,
100
);
visionCornerSeverityPct =
constrain(
parsed >= 5
? cornerSeverity
: 0,
0,
100
);
visionPacketValid = true;
lastVisionPacketTime =
millis();
return true;
}
void getWokwiCornerTestCommand(
int &steerPct,
int &speedPct,
int &cornerSeverityPct
)
{
steerPct =
WOKWI_VISION_STEER_PCT;
speedPct =
WOKWI_VISION_SPEED_PCT;
cornerSeverityPct =
WOKWI_VISION_CORNER_SEVERITY_PCT;
if (!WOKWI_AUTO_CORNER_SPEED_TEST)
{
return;
}
if (!wokwiCornerTestStarted)
{
wokwiCornerTestStarted = true;
wokwiCornerTestStartTime = millis();
}
unsigned long elapsed =
millis() -
wokwiCornerTestStartTime;
// 0-3 s: STRAIGHT
if (elapsed < CORNER_TEST_GENTLE_MS)
{
steerPct = 0;
speedPct = 100;
cornerSeverityPct = 0;
return;
}
// 3-6 s: GENTLE RIGHT
if (elapsed < CORNER_TEST_MEDIUM_MS)
{
steerPct = 20;
speedPct = 100;
cornerSeverityPct = 25;
return;
}
// 6-9 s: MEDIUM LEFT
if (elapsed < CORNER_TEST_SHARP_MS)
{
steerPct = -40;
speedPct = 100;
cornerSeverityPct = 50;
return;
}
// 9-12 s: SHARP RIGHT
if (elapsed < CORNER_TEST_VERY_SHARP_MS)
{
steerPct = 60;
speedPct = 100;
cornerSeverityPct = 75;
return;
}
// 12-15 s: VERY SHARP LEFT
if (elapsed < CORNER_TEST_END_MS)
{
steerPct = -75;
speedPct = 100;
cornerSeverityPct = 100;
return;
}
// >15 s: STRAIGHT AGAIN
steerPct = 0;
speedPct = 100;
cornerSeverityPct = 0;
}
void updateVisionSerial()
{
// ===================================================
// WOKWI INTERNAL VISION MODE
// ===================================================
//
// This bypasses the custom Pi UART chip entirely.
// It tests the SAME downstream STM32 navigation logic
// that the real Pi packets will use later.
// ===================================================
if (USE_WOKWI_INTERNAL_VISION_SIM)
{
unsigned long now =
millis();
if (
now - previousWokwiVisionTime
<
WOKWI_VISION_INTERVAL_MS
)
{
return;
}
previousWokwiVisionTime =
now;
if (!WOKWI_VISION_ONLINE)
{
// Send nothing. The normal 250 ms timeout will
// naturally change VISION STATUS to OFFLINE / STALE.
return;
}
int simulatedSteerPct = 0;
int simulatedSpeedPct = 100;
int simulatedCornerSeverityPct = 0;
getWokwiCornerTestCommand(
simulatedSteerPct,
simulatedSpeedPct,
simulatedCornerSeverityPct
);
visionSteerPct =
constrain(
simulatedSteerPct,
-100,
100
);
visionSpeedPct =
constrain(
simulatedSpeedPct,
VISION_MIN_SPEED_PCT,
VISION_MAX_SPEED_PCT
);
visionCornerSeverityPct =
constrain(
simulatedCornerSeverityPct,
0,
100
);
visionConfidencePct =
constrain(
WOKWI_VISION_CONFIDENCE_PCT,
0,
100
);
visionBridgeConfidencePct =
constrain(
wokwiSimulatedBridgeConfidencePct,
0,
100
);
visionPacketValid = true;
lastVisionPacketTime =
now;
// Keep the existing diagnostics useful.
snprintf(
piLastRawPacket,
sizeof(piLastRawPacket),
"SIM,V,%d,%d,%d,%d,%d",
visionSteerPct,
visionSpeedPct,
visionConfidencePct,
visionBridgeConfidencePct,
visionCornerSeverityPct
);
piGoodPacketCount++;
return;
}
// ===================================================
// PHYSICAL RASPBERRY PI UART MODE
// ===================================================
//
// Set USE_WOKWI_INTERNAL_VISION_SIM = false to use this
// path. The real Pi sends:
//
// V,<steerPct>,<speedPct>,<confidencePct>,<bridgeConfidencePct>,<cornerSeverityPct>\n
//
// Older 4-field and 5-field packets remain accepted.
// ===================================================
while (PiSerial.available() > 0)
{
char c =
(char)PiSerial.read();
piRxByteCount++;
if (
c == '\n'
||
c == '\r'
)
{
if (visionRxIndex > 0)
{
visionRxBuffer[
visionRxIndex
] = '\0';
strncpy(
piLastRawPacket,
visionRxBuffer,
sizeof(piLastRawPacket) - 1
);
piLastRawPacket[
sizeof(piLastRawPacket) - 1
] = '\0';
if (
parseVisionPacket(
visionRxBuffer
)
)
{
piGoodPacketCount++;
}
else
{
piBadPacketCount++;
}
visionRxIndex = 0;
}
continue;
}
if (
visionRxIndex <
sizeof(visionRxBuffer) - 1
)
{
visionRxBuffer[
visionRxIndex++
] = c;
}
else
{
visionRxIndex = 0;
piBadPacketCount++;
}
}
}
// =====================================================
// CAMERA PREDICTIVE MOTOR TARGETS
// =====================================================
void applyVisionGuidanceAtBase(
float requestedBaseRPM
)
{
float baseRPM =
min(
getVisionBaseRPM(),
requestedBaseRPM
);
float normalizedSteer =
(float)visionSteerPct /
100.0f;
float correctionRPM =
VISION_MAX_STEER_RPM *
normalizedSteer;
// Positive steering means RIGHT:
// left wheel faster, right wheel slower.
leftTargetRPM =
baseRPM +
correctionRPM;
rightTargetRPM =
baseRPM -
correctionRPM;
leftTargetRPM =
constrain(
leftTargetRPM,
MIN_TARGET_RPM,
MAX_TARGET_RPM
);
rightTargetRPM =
constrain(
rightTargetRPM,
MIN_TARGET_RPM,
MAX_TARGET_RPM
);
}
void applyVisionGuidance()
{
applyVisionGuidanceAtBase(
NORMAL_BASE_RPM
);
}
// =====================================================
// ROAD-BOUNDARY SENSORS
// =====================================================
void readBoundarySensors()
{
for (int i = 0; i < 8; i++)
{
sensor[i] =
digitalRead(IR_PINS[i]);
}
}
void analyseRoadBoundaries()
{
float weightedSum = 0.0f;
activeBoundarySensors = 0;
leftBoundarySeverity = 0.0f;
rightBoundarySeverity = 0.0f;
boundarySeverity = 0.0f;
bool leftActive = false;
bool rightActive = false;
for (int i = 0; i < 8; i++)
{
if (sensor[i] != HIGH)
continue;
float weight =
BOUNDARY_STEER_WEIGHTS[i];
float severity =
fabsf(weight);
weightedSum += weight;
activeBoundarySensors++;
if (severity > boundarySeverity)
boundarySeverity = severity;
if (i <= 3)
{
leftActive = true;
if (severity > leftBoundarySeverity)
leftBoundarySeverity = severity;
}
else
{
rightActive = true;
if (severity > rightBoundarySeverity)
rightBoundarySeverity = severity;
}
}
boundaryDetected =
activeBoundarySensors > 0;
centreBoundaryDetected =
(
sensor[3] == HIGH
||
sensor[4] == HIGH
);
bothSidesBoundaryDetected =
leftActive && rightActive;
if (!boundaryDetected)
{
boundarySteerError = 0.0f;
return;
}
boundarySteerError =
weightedSum /
(float)activeBoundarySensors;
// Keep a useful directional memory. Do not replace it
// with an almost perfectly balanced / ambiguous value.
if (
fabsf(boundarySteerError)
>
0.25f
)
{
lastBoundarySteerError =
boundarySteerError;
}
}
// =====================================================
// TOF READ
// =====================================================
bool readToFDistance(
uint8_t address,
uint16_t &distanceMM
)
{
Wire.beginTransmission(address);
Wire.write(
(uint8_t)(
SIM_RANGE_REGISTER >> 8
)
);
Wire.write(
(uint8_t)(
SIM_RANGE_REGISTER & 0xFF
)
);
// NORMAL STOP TRANSACTION
uint8_t result =
Wire.endTransmission();
if (result != 0)
return false;
uint8_t received =
Wire.requestFrom(
address,
(uint8_t)2
);
if (
received != 2 ||
Wire.available() < 2
)
{
return false;
}
uint8_t highByte =
Wire.read();
uint8_t lowByte =
Wire.read();
distanceMM =
((uint16_t)highByte << 8)
|
lowByte;
// ===================================================
// VL53L1X SENSOR-FAULT DETECTION
//
// The Wokwi fault simulator returns 0xFFFF (65535)
// when Sensor Fault = 1. A zero reading is also
// treated as invalid.
// ===================================================
if (
distanceMM == 0
||
distanceMM >= 5000
)
{
return false;
}
return true;
}
// =====================================================
// TOF + TTC UPDATE
// =====================================================
void updateToF()
{
unsigned long now =
millis();
if (
now - previousToFTime <
TOF_INTERVAL_MS
)
{
return;
}
float dt =
(now - previousToFTime)
/
1000.0f;
previousToFTime = now;
uint16_t newLeft = 0;
uint16_t newRight = 0;
bool leftOK = false;
bool rightOK = false;
if (USE_WOKWI_INTERNAL_TOF_SIM)
{
// Internal ToF mode is retained only as a fallback.
// The current Wokwi race build keeps this FALSE so the
// custom VL53L1X distance sliders are used.
newLeft = 1500;
newRight = 1500;
leftOK = true;
rightOK = true;
}
else
{
// Interactive Wokwi mode:
// read the actual custom VL53L1X chips.
// Moving either distance slider changes these values.
leftOK =
readToFDistance(
LEFT_TOF_ADDR,
newLeft
);
rightOK =
readToFDistance(
RIGHT_TOF_ADDR,
newRight
);
}
if (!leftOK || !rightOK)
{
tofValid = false;
previousToFValid = false;
return;
}
tofValid = true;
leftDistance = newLeft;
rightDistance = newRight;
if (
previousToFValid &&
dt > 0.001f
)
{
leftClosingSpeed =
(
(float)previousLeftDistance -
(float)leftDistance
)
/
dt;
rightClosingSpeed =
(
(float)previousRightDistance -
(float)rightDistance
)
/
dt;
if (
leftClosingSpeed >
MIN_CLOSING_SPEED_MM_S
)
{
leftTTC =
(float)leftDistance /
leftClosingSpeed;
}
else
{
leftTTC = -1.0f;
}
if (
rightClosingSpeed >
MIN_CLOSING_SPEED_MM_S
)
{
rightTTC =
(float)rightDistance /
rightClosingSpeed;
}
else
{
rightTTC = -1.0f;
}
}
else
{
leftClosingSpeed = 0.0f;
rightClosingSpeed = 0.0f;
leftTTC = -1.0f;
rightTTC = -1.0f;
}
previousLeftDistance =
leftDistance;
previousRightDistance =
rightDistance;
previousToFValid = true;
}
// =====================================================
// INA219 REGISTER READ
//
// Uses WRITE -> STOP -> READ -> STOP.
//
// This matches the stable transaction style already used
// by the shared Wokwi I2C simulation.
// =====================================================
bool readINA219Register(
uint8_t reg,
uint16_t &value
)
{
Wire.beginTransmission(
INA219_ADDR
);
Wire.write(reg);
if (
Wire.endTransmission()
!= 0
)
{
return false;
}
uint8_t received =
Wire.requestFrom(
INA219_ADDR,
(uint8_t)2
);
if (
received != 2 ||
Wire.available() < 2
)
{
return false;
}
uint8_t highByte =
Wire.read();
uint8_t lowByte =
Wire.read();
value =
((uint16_t)highByte << 8)
|
lowByte;
return true;
}
// =====================================================
// INA219 UPDATE
// =====================================================
void updateINA219(
bool forceRead = false
)
{
unsigned long now =
millis();
if (
!forceRead &&
now - previousINATime <
INA_INTERVAL_MS
)
{
return;
}
previousINATime = now;
if (USE_WOKWI_INTERNAL_INA_SIM)
{
inaValid =
WOKWI_INA_ONLINE;
if (!inaValid)
{
motorStallDetected = false;
stallConditionStart = 0;
return;
}
busVoltageV =
WOKWI_BUS_VOLTAGE_V;
motorCurrentMA =
WOKWI_MOTOR_CURRENT_MA;
return;
}
uint16_t rawBusVoltage = 0;
uint16_t rawCurrent = 0;
bool busOK =
readINA219Register(
INA219_REG_BUS_VOLTAGE,
rawBusVoltage
);
bool currentOK =
readINA219Register(
INA219_REG_CURRENT,
rawCurrent
);
if (!busOK || !currentOK)
{
inaValid = false;
motorStallDetected = false;
stallConditionStart = 0;
return;
}
inaValid = true;
// INA219-style bus voltage register:
// voltage bits are [15:3], 4 mV per bit.
uint16_t busVoltageMV =
(rawBusVoltage >> 3)
* 4U;
busVoltageV =
(float)busVoltageMV /
1000.0f;
// Wokwi behavioural-model convention:
// 1 raw current count = 1 mA.
motorCurrentMA =
(float)(
(int16_t)rawCurrent
);
}
// =====================================================
// MOTOR STALL DETECTION
// =====================================================
//
// A stall is only declared when ALL are true:
//
// 1. INA219 current is high.
// 2. The controller is commanding meaningful motion.
// 3. Actual wheel speed is very low.
// 4. The condition persists for STALL_CONFIRM_MS.
//
// The time confirmation prevents startup or brief
// transients from immediately causing a false stall.
// =====================================================
void updateMotorStallDetection()
{
// ===================================================
// HARD STALL LATCH
// ===================================================
//
// Once a real stall is confirmed, current falling back
// to normal MUST NOT automatically restart the robot.
//
// The robot stays stopped until the operator deliberately
// cycles the MAIN switch:
//
// MAIN ON -> stall detected -> latched stop
// MAIN OFF -> latch is cleared while motors are disabled
// MAIN ON -> autonomous control may start again
//
// This is safer for a fully autonomous racing robot because
// it prevents repeated automatic attempts against a jammed
// wheel or trapped drivetrain.
// ===================================================
if (motorStallLatched)
{
motorStallDetected = true;
return;
}
if (!inaValid)
{
motorStallDetected = false;
stallCandidateMask = 0;
stallLatchedSideMask = 0;
stallConditionStart = 0;
return;
}
// Do not diagnose a mechanical stall from corrupt
// encoder samples.
if (!leftRPMValid || !rightRPMValid)
{
motorStallDetected = false;
stallCandidateMask = 0;
stallConditionStart = 0;
return;
}
// ===================================================
// PER-WHEEL STALL CHECK
// ===================================================
//
// Previous logic averaged the two wheel RPM values.
// That misses a one-wheel stall:
//
// left = 0 RPM
// right = 38 RPM
// average = 19 RPM
//
// 19 RPM is above the 5 RPM stall threshold, so the
// old controller incorrectly concluded "NO STALL".
//
// We now evaluate each wheel independently.
// ===================================================
bool highCurrent =
motorCurrentMA >=
STALL_CURRENT_MA;
bool leftCommanded =
fabsf(
leftRampedTargetRPM
)
>
STALL_COMMAND_RPM_THRESHOLD;
bool rightCommanded =
fabsf(
rightRampedTargetRPM
)
>
STALL_COMMAND_RPM_THRESHOLD;
bool leftTooSlow =
fabsf(
leftRPM
)
<
STALL_RPM_THRESHOLD;
bool rightTooSlow =
fabsf(
rightRPM
)
<
STALL_RPM_THRESHOLD;
bool leftStallCondition =
highCurrent
&&
leftCommanded
&&
leftTooSlow;
bool rightStallCondition =
highCurrent
&&
rightCommanded
&&
rightTooSlow;
stallCandidateMask = 0;
if (leftStallCondition)
{
stallCandidateMask |= 0x01;
}
if (rightStallCondition)
{
stallCandidateMask |= 0x02;
}
bool stallCondition =
stallCandidateMask != 0;
unsigned long now =
millis();
if (!stallCondition)
{
motorStallDetected = false;
stallConditionStart = 0;
return;
}
if (stallConditionStart == 0)
{
stallConditionStart = now;
motorStallDetected = false;
return;
}
if (
now - stallConditionStart >=
STALL_CONFIRM_MS
)
{
motorStallLatched = true;
motorStallDetected = true;
stallLatchedSideMask =
stallCandidateMask;
}
else
{
motorStallDetected = false;
}
}
void resetMotorStallLatchWhileSystemOff()
{
// This function must only be called while MAIN SWITCH = OFF.
//
// The motor driver is already disabled in SYSTEM OFF, so it
// is safe to clear the software latch here. The robot cannot
// move again until the operator deliberately switches MAIN ON.
motorStallLatched = false;
motorStallDetected = false;
stallCandidateMask = 0;
stallLatchedSideMask = 0;
stallConditionStart = 0;
}
const char *stallSideName()
{
uint8_t mask =
motorStallLatched
? stallLatchedSideMask
: stallCandidateMask;
if (mask == 0x03)
{
return "BOTH";
}
if (mask == 0x01)
{
return "LEFT";
}
if (mask == 0x02)
{
return "RIGHT";
}
return "NONE";
}
// =====================================================
// CURRENT STATUS TEXT
// =====================================================
const char *currentStatusName()
{
if (!inaValid)
return "SENSOR ERROR";
if (motorStallDetected)
return "STALL";
if (
motorCurrentMA >=
HIGH_CURRENT_MA
)
{
return "HIGH";
}
return "NORMAL";
}
// =====================================================
// MPU WRITE
// =====================================================
bool writeMPURegister(
uint8_t reg,
uint8_t value
)
{
Wire.beginTransmission(
MPU_ADDR
);
Wire.write(reg);
Wire.write(value);
return
Wire.endTransmission() == 0;
}
// =====================================================
// MPU READ BYTE
//
// IMPORTANT:
// NO REPEATED START.
//
// We deliberately use:
// WRITE -> STOP
// READ -> STOP
//
// because this is stable with the current Wokwi
// shared I2C simulation.
// =====================================================
bool readMPUByte(
uint8_t reg,
uint8_t &value
)
{
Wire.beginTransmission(
MPU_ADDR
);
Wire.write(reg);
if (
Wire.endTransmission()
!= 0
)
{
return false;
}
uint8_t received =
Wire.requestFrom(
MPU_ADDR,
(uint8_t)1
);
if (
received != 1 ||
Wire.available() < 1
)
{
return false;
}
value =
Wire.read();
return true;
}
// =====================================================
// MPU INITIALISATION
// =====================================================
bool initialiseMPU()
{
if (USE_WOKWI_INTERNAL_IMU_SIM)
{
gyroZ = WOKWI_GYRO_Z_DPS;
return WOKWI_IMU_ONLINE;
}
if (
!writeMPURegister(
MPU_PWR_MGMT_1,
0x00
)
)
{
return false;
}
if (
!writeMPURegister(
MPU_GYRO_CONFIG,
0x00
)
)
{
return false;
}
uint8_t who = 0;
if (
!readMPUByte(
MPU_WHO_AM_I,
who
)
)
{
return false;
}
return who == 0x68;
}
// =====================================================
// READ GYRO Z
//
// ALSO USES NORMAL STOP TRANSACTIONS.
// =====================================================
bool readGyroZ()
{
if (USE_WOKWI_INTERNAL_IMU_SIM)
{
gyroZ =
WOKWI_GYRO_Z_DPS;
pitchDeg =
wokwiSimulatedPitchDeg;
return
WOKWI_IMU_ONLINE;
}
// Physical MPU6050:
// read accel XYZ + temp + gyro XYZ in one 14-byte burst.
Wire.beginTransmission(
MPU_ADDR
);
Wire.write(
MPU_ACCEL_XOUT_H
);
if (
Wire.endTransmission()
!= 0
)
{
return false;
}
uint8_t received =
Wire.requestFrom(
MPU_ADDR,
(uint8_t)14
);
if (
received != 14
||
Wire.available() < 14
)
{
return false;
}
int16_t rawAx =
((int16_t)Wire.read() << 8)
|
Wire.read();
int16_t rawAy =
((int16_t)Wire.read() << 8)
|
Wire.read();
int16_t rawAz =
((int16_t)Wire.read() << 8)
|
Wire.read();
// Temperature (unused).
Wire.read();
Wire.read();
// Gyro X and Y (unused here).
Wire.read();
Wire.read();
Wire.read();
Wire.read();
int16_t rawGz =
((int16_t)Wire.read() << 8)
|
Wire.read();
gyroZ =
(float)rawGz /
GYRO_SENSITIVITY;
// Assumed physical orientation:
// X points forward, Z points upward.
// Change axis/sign after real mounting calibration if needed.
float ax = (float)rawAx;
float ay = (float)rawAy;
float az = (float)rawAz;
float measuredPitch =
atan2f(
-ax,
sqrtf(
ay * ay
+
az * az
)
)
*
180.0f
/
PI;
// Low-pass pitch for vibration/noise.
pitchDeg =
0.80f * pitchDeg
+
0.20f * measuredPitch;
return true;
}
// =====================================================
// TTC HELPERS
// =====================================================
float getMinimumTTC()
{
bool leftValid =
leftTTC > 0.0f;
bool rightValid =
rightTTC > 0.0f;
if (leftValid && rightValid)
{
return
leftTTC < rightTTC
?
leftTTC
:
rightTTC;
}
if (leftValid)
return leftTTC;
if (rightValid)
return rightTTC;
return 999.0f;
}
// =====================================================
// AVOIDANCE DIRECTION
//
// -1 = LEFT
// +1 = RIGHT
// 0 = UNDETERMINED
// =====================================================
int chooseAvoidanceDirection()
{
int direction = 0;
// First choose the safer side using TTC when available.
if (
leftTTC > 0.0f
&&
rightTTC > 0.0f
)
{
if (
fabsf(
leftTTC -
rightTTC
)
>
0.15f
)
{
if (leftTTC < rightTTC)
direction = +1; // obstacle closes sooner on left -> go right
else
direction = -1; // obstacle closes sooner on right -> go left
}
}
else if (leftTTC > 0.0f)
{
direction = +1;
}
else if (rightTTC > 0.0f)
{
direction = -1;
}
// If TTC did not decide, use the measured distances.
if (direction == 0)
{
int difference =
(int)leftDistance -
(int)rightDistance;
if (
abs(difference) >
SIDE_TOLERANCE_MM
)
{
if (leftDistance < rightDistance)
direction = +1;
else
direction = -1;
}
}
// ===================================================
// ROAD-BOUNDARY VETO
// ===================================================
//
// Do not avoid an obstacle by steering directly into
// a nearby road boundary.
//
// direction -1 = LEFT
// direction +1 = RIGHT
//
// Severity >= 2 means the boundary has reached beyond
// the far-outside sensor and we treat that side as
// unsafe for an obstacle-avoidance turn.
// ===================================================
if (
direction < 0
&&
leftBoundarySeverity >= 2.0f
)
{
return 0;
}
if (
direction > 0
&&
rightBoundarySeverity >= 2.0f
)
{
return 0;
}
return direction;
}
// =====================================================
// SKID DETECTION
// =====================================================
void updateSkidDetection()
{
if (!leftRPMValid || !rightRPMValid)
{
possibleSkid = false;
return;
}
float rpmDifference =
fabsf(
leftRPM -
rightRPM
);
float averageRPM =
(
fabsf(leftRPM) +
fabsf(rightRPM)
)
/
2.0f;
possibleSkid =
(
averageRPM > 10.0f
&&
rpmDifference < 3.0f
&&
fabsf(gyroZ) > 25.0f
);
}
// =====================================================
// AUTONOMOUS BRIDGE PHASE ESTIMATION
// =====================================================
const char *bridgePhaseName()
{
switch (bridgePhase)
{
case BRIDGE_NONE: return "NONE";
case BRIDGE_APPROACH: return "APPROACH";
case BRIDGE_CLIMB: return "CLIMB";
case BRIDGE_CREST: return "CREST";
case BRIDGE_DESCENT: return "DESCENT";
case BRIDGE_EXIT: return "EXIT";
}
return "INVALID";
}
void updateBridgePhase()
{
if (pitchDeg > maximumBridgeClimbPitchDeg)
{
maximumBridgeClimbPitchDeg =
pitchDeg;
}
switch (bridgePhase)
{
case BRIDGE_NONE:
maximumBridgeClimbPitchDeg = 0.0f;
bridgeExitStartTime = 0;
if (
visionIsTrusted()
&&
visionBridgeConfidencePct >=
BRIDGE_CAMERA_ENTER_CONFIDENCE_PCT
)
{
bridgePhase = BRIDGE_APPROACH;
return;
}
// IMU fallback if camera prediction is missed.
if (
imuValid
&&
pitchDeg >=
BRIDGE_CLIMB_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_CLIMB;
maximumBridgeClimbPitchDeg = pitchDeg;
return;
}
if (
imuValid
&&
pitchDeg <=
BRIDGE_DESCENT_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_DESCENT;
return;
}
break;
case BRIDGE_APPROACH:
if (
imuValid
&&
pitchDeg >=
BRIDGE_CLIMB_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_CLIMB;
maximumBridgeClimbPitchDeg = pitchDeg;
return;
}
// Cancel false camera bridge detection.
if (
visionBridgeConfidencePct <
BRIDGE_CAMERA_CLEAR_CONFIDENCE_PCT
&&
fabsf(pitchDeg) <
BRIDGE_FLAT_PITCH_DEG
)
{
bridgePhase = BRIDGE_NONE;
return;
}
break;
case BRIDGE_CLIMB:
if (
pitchDeg <=
BRIDGE_DESCENT_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_DESCENT;
return;
}
if (
maximumBridgeClimbPitchDeg >=
BRIDGE_CLIMB_ENTER_PITCH_DEG
&&
fabsf(pitchDeg) <=
BRIDGE_CREST_PITCH_DEG
)
{
bridgePhase = BRIDGE_CREST;
return;
}
break;
case BRIDGE_CREST:
if (
pitchDeg <=
BRIDGE_DESCENT_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_DESCENT;
return;
}
if (
pitchDeg >=
BRIDGE_CLIMB_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_CLIMB;
return;
}
break;
case BRIDGE_DESCENT:
if (
fabsf(pitchDeg) <=
BRIDGE_FLAT_PITCH_DEG
)
{
bridgePhase = BRIDGE_EXIT;
bridgeExitStartTime = millis();
return;
}
break;
case BRIDGE_EXIT:
if (
pitchDeg <=
BRIDGE_DESCENT_ENTER_PITCH_DEG
)
{
bridgePhase = BRIDGE_DESCENT;
bridgeExitStartTime = 0;
return;
}
if (
fabsf(pitchDeg) <=
BRIDGE_FLAT_PITCH_DEG
&&
visionBridgeConfidencePct <
BRIDGE_CAMERA_CLEAR_CONFIDENCE_PCT
)
{
if (
millis() -
bridgeExitStartTime >=
BRIDGE_EXIT_CONFIRM_MS
)
{
bridgePhase = BRIDGE_NONE;
maximumBridgeClimbPitchDeg = 0.0f;
bridgeExitStartTime = 0;
return;
}
}
else
{
bridgeExitStartTime = millis();
}
break;
}
}
// =====================================================
// BRIDGE-SAFE CAMERA STEERING
// =====================================================
void applyBridgeGuidance(
float baseRPM,
float maximumSteerRPM
)
{
if (!visionIsTrusted())
{
applyRoadKeeping(
baseRPM
);
return;
}
float safeBridgeBaseRPM =
min(
baseRPM,
getVisionCornerSpeedCapRPM()
);
float normalizedSteer =
constrain(
(float)visionSteerPct /
100.0f,
-1.0f,
1.0f
);
float correctionRPM =
maximumSteerRPM *
normalizedSteer;
leftTargetRPM =
constrain(
safeBridgeBaseRPM + correctionRPM,
MIN_TARGET_RPM,
min(
MAX_TARGET_RPM,
safeBridgeBaseRPM + maximumSteerRPM
)
);
rightTargetRPM =
constrain(
safeBridgeBaseRPM - correctionRPM,
MIN_TARGET_RPM,
min(
MAX_TARGET_RPM,
safeBridgeBaseRPM + maximumSteerRPM
)
);
}
// =====================================================
// STATE DECISION
// =====================================================
void determineNavigationState()
{
// ===================================================
// 1. ABSOLUTE SAFETY: E-STOP
// ===================================================
if (emergencyStopActive)
{
currentState =
STATE_EMERGENCY_STOP;
return;
}
// ===================================================
// 2. MAIN SWITCH OFF
// ===================================================
if (!mainSwitchOn)
{
currentState =
STATE_SYSTEM_OFF;
return;
}
// ===================================================
// 3. MOTOR / ELECTRICAL PROTECTION
// ===================================================
if (motorStallDetected)
{
currentState =
STATE_MOTOR_STALL;
return;
}
// ===================================================
// 3. COLLISION SAFETY: ToF + TTC
// ===================================================
//
// Other race cars and track walls are dynamic hazards.
// TTC makes the decision depend on closing rate as well
// as raw distance.
// ===================================================
if (tofValid)
{
uint16_t nearestDistance =
leftDistance < rightDistance
?
leftDistance
:
rightDistance;
float minimumTTC =
getMinimumTTC();
int avoidDirection =
chooseAvoidanceDirection();
// Critical collision risk
if (
nearestDistance <= CRITICAL_RANGE_MM
||
minimumTTC <= TTC_CRITICAL_S
)
{
if (avoidDirection > 0)
currentState = STATE_CRITICAL_AVOID_RIGHT;
else if (avoidDirection < 0)
currentState = STATE_CRITICAL_AVOID_LEFT;
else
currentState = STATE_CRITICAL_STOP;
return;
}
// Active avoidance
if (
nearestDistance <= AVOID_RANGE_MM
||
minimumTTC <= TTC_AVOID_S
)
{
if (avoidDirection > 0)
currentState = STATE_AVOID_RIGHT;
else if (avoidDirection < 0)
currentState = STATE_AVOID_LEFT;
else
currentState = STATE_NO_SAFE_PATH;
return;
}
// Early caution
if (
nearestDistance <= CAUTION_RANGE_MM
||
minimumTTC <= TTC_CAUTION_S
)
{
currentState =
STATE_CAUTION;
return;
}
}
// ===================================================
// 4. SKID / STABILITY PROTECTION
// ===================================================
if (possibleSkid)
{
currentState =
STATE_SKID_CAUTION;
return;
}
// ===================================================
// 5. IR ROAD-BOUNDARY OVERRIDE
// ===================================================
//
// Camera predicts what is coming.
// IR protects what is happening under the vehicle NOW.
// ===================================================
if (boundaryDetected)
{
if (
centreBoundaryDetected
&&
fabsf(boundarySteerError) <= 0.25f
)
{
currentState =
STATE_BOUNDARY_ESCAPE;
return;
}
if (boundarySteerError > 0.25f)
{
currentState =
STATE_BOUNDARY_CORRECT_RIGHT;
return;
}
if (boundarySteerError < -0.25f)
{
currentState =
STATE_BOUNDARY_CORRECT_LEFT;
return;
}
currentState =
STATE_BOUNDARY_CAUTION;
return;
}
// ===================================================
// 6. ToF FAULT -> DEGRADED SPEED
// ===================================================
//
// Camera + IR can still navigate, but full racing speed
// is not allowed without the car/wall collision layer.
// ===================================================
if (!tofValid)
{
currentState =
STATE_SENSOR_FAULT;
return;
}
// ===================================================
// 8. AUTONOMOUS BRIDGE HANDLING
// ===================================================
switch (bridgePhase)
{
case BRIDGE_APPROACH:
currentState = STATE_BRIDGE_APPROACH;
return;
case BRIDGE_CLIMB:
currentState = STATE_BRIDGE_CLIMB;
return;
case BRIDGE_CREST:
currentState = STATE_BRIDGE_CREST;
return;
case BRIDGE_DESCENT:
currentState = STATE_BRIDGE_DESCENT;
return;
case BRIDGE_EXIT:
currentState = STATE_BRIDGE_EXIT;
return;
case BRIDGE_NONE:
break;
}
// ===================================================
// 9. CAMERA PREDICTIVE LOOKAHEAD
// ===================================================
if (visionIsTrusted())
{
currentState =
STATE_VISION_DRIVE;
return;
}
// Camera missing / stale / low confidence.
currentState =
STATE_VISION_FALLBACK;
}
// =====================================================
// CHOOSE BOUNDARY ESCAPE DIRECTION
// =====================================================
//
// Return:
// -1 = turn LEFT
// +1 = turn RIGHT
//
// This is mainly for an ambiguous centre-boundary hit
// such as 00011000. We first use directional memory,
// then use ToF free space, then use a deterministic
// right-turn fallback for the Wokwi test.
// =====================================================
int chooseBoundaryEscapeDirection()
{
if (lastBoundarySteerError > 0.25f)
return +1;
if (lastBoundarySteerError < -0.25f)
return -1;
if (tofValid)
{
if (
leftDistance >
rightDistance + SIDE_TOLERANCE_MM
)
{
return -1;
}
if (
rightDistance >
leftDistance + SIDE_TOLERANCE_MM
)
{
return +1;
}
}
return +1;
}
// =====================================================
// ROAD KEEPING / BOUNDARY AVOIDANCE
// =====================================================
void applyRoadKeeping(
float requestedBaseRPM
)
{
// Clear road:
// all IR sensors are over the drivable road surface.
if (!boundaryDetected)
{
leftTargetRPM =
requestedBaseRPM;
rightTargetRPM =
requestedBaseRPM;
return;
}
// Reduce speed as the boundary moves closer to the
// centre of the sensor array.
float baseRPM =
requestedBaseRPM;
if (boundarySeverity >= 3.5f)
{
baseRPM =
min(
baseRPM,
BOUNDARY_CENTRE_RPM
);
}
else if (boundarySeverity >= 2.5f)
{
baseRPM =
min(
baseRPM,
BOUNDARY_INNER_RPM
);
}
else if (bothSidesBoundaryDetected)
{
baseRPM =
min(
baseRPM,
BOUNDARY_CAUTION_RPM
);
}
float correction =
K_BOUNDARY_STEER *
boundarySteerError;
// Positive correction means the boundary is on the
// LEFT, therefore the left wheel speeds up and the
// right wheel slows down to turn the robot RIGHT.
leftTargetRPM =
baseRPM +
correction;
rightTargetRPM =
baseRPM -
correction;
leftTargetRPM =
constrain(
leftTargetRPM,
MIN_TARGET_RPM,
MAX_TARGET_RPM
);
rightTargetRPM =
constrain(
rightTargetRPM,
MIN_TARGET_RPM,
MAX_TARGET_RPM
);
}
// =====================================================
// BOUNDARY ESCAPE
// =====================================================
void applyBoundaryEscape()
{
int direction =
chooseBoundaryEscapeDirection();
if (direction > 0)
{
// Turn RIGHT strongly.
leftTargetRPM = AVOID_FAST_RPM;
rightTargetRPM = CRITICAL_AVOID_SLOW_RPM;
}
else
{
// Turn LEFT strongly.
leftTargetRPM = CRITICAL_AVOID_SLOW_RPM;
rightTargetRPM = AVOID_FAST_RPM;
}
}
// =====================================================
// APPLY NAVIGATION TARGETS
// =====================================================
void applyNavigationTargets()
{
switch (currentState)
{
case STATE_EMERGENCY_STOP:
case STATE_SYSTEM_OFF:
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
break;
case STATE_VISION_DRIVE:
applyVisionGuidance();
break;
case STATE_VISION_FALLBACK:
applyRoadKeeping(
VISION_FALLBACK_RPM
);
break;
case STATE_ROAD_DRIVE:
applyRoadKeeping(
getVisionBaseRPM()
);
break;
case STATE_BOUNDARY_CORRECT_LEFT:
applyRoadKeeping(
getVisionBaseRPM()
);
break;
case STATE_BOUNDARY_CORRECT_RIGHT:
applyRoadKeeping(
getVisionBaseRPM()
);
break;
case STATE_BOUNDARY_CAUTION:
applyRoadKeeping(
min(
getVisionBaseRPM(),
BOUNDARY_CAUTION_RPM
)
);
break;
case STATE_BOUNDARY_ESCAPE:
applyBoundaryEscape();
break;
case STATE_CAUTION:
// Preserve road/camera guidance while reducing speed.
if (boundaryDetected)
{
applyRoadKeeping(
CAUTION_BASE_RPM
);
}
else if (visionIsTrusted())
{
applyVisionGuidanceAtBase(
CAUTION_BASE_RPM
);
}
else
{
leftTargetRPM =
CAUTION_BASE_RPM;
rightTargetRPM =
CAUTION_BASE_RPM;
}
break;
case STATE_AVOID_LEFT:
leftTargetRPM = AVOID_SLOW_RPM;
rightTargetRPM = AVOID_FAST_RPM;
break;
case STATE_AVOID_RIGHT:
leftTargetRPM = AVOID_FAST_RPM;
rightTargetRPM = AVOID_SLOW_RPM;
break;
case STATE_CRITICAL_AVOID_LEFT:
leftTargetRPM = CRITICAL_AVOID_SLOW_RPM;
rightTargetRPM = CRITICAL_AVOID_FAST_RPM;
break;
case STATE_CRITICAL_AVOID_RIGHT:
leftTargetRPM = CRITICAL_AVOID_FAST_RPM;
rightTargetRPM = CRITICAL_AVOID_SLOW_RPM;
break;
case STATE_CRITICAL_STOP:
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
break;
case STATE_NO_SAFE_PATH:
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
break;
case STATE_MOTOR_STALL:
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
break;
case STATE_SKID_CAUTION:
if (boundaryDetected)
{
applyRoadKeeping(
SKID_BASE_RPM
);
}
else if (visionIsTrusted())
{
applyVisionGuidanceAtBase(
SKID_BASE_RPM
);
}
else
{
leftTargetRPM =
SKID_BASE_RPM;
rightTargetRPM =
SKID_BASE_RPM;
}
break;
case STATE_BRIDGE_APPROACH:
applyBridgeGuidance(
BRIDGE_APPROACH_RPM,
BRIDGE_APPROACH_STEER_RPM
);
break;
case STATE_BRIDGE_CLIMB:
applyBridgeGuidance(
BRIDGE_CLIMB_RPM,
BRIDGE_CLIMB_STEER_RPM
);
break;
case STATE_BRIDGE_CREST:
applyBridgeGuidance(
BRIDGE_CREST_RPM,
BRIDGE_CREST_STEER_RPM
);
break;
case STATE_BRIDGE_DESCENT:
applyBridgeGuidance(
BRIDGE_DESCENT_RPM,
BRIDGE_DESCENT_STEER_RPM
);
break;
case STATE_BRIDGE_EXIT:
applyBridgeGuidance(
BRIDGE_EXIT_RPM,
BRIDGE_EXIT_STEER_RPM
);
break;
case STATE_SENSOR_FAULT:
if (boundaryDetected)
{
applyRoadKeeping(
SENSOR_FAULT_RPM
);
}
else if (visionIsTrusted())
{
applyVisionGuidanceAtBase(
SENSOR_FAULT_RPM
);
}
else
{
leftTargetRPM =
SENSOR_FAULT_RPM;
rightTargetRPM =
SENSOR_FAULT_RPM;
}
break;
default:
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
break;
}
}
// =====================================================
// STATE CHANGE HANDLING
// =====================================================
void handleStateChange()
{
if (
currentState ==
previousState
)
{
return;
}
leftIntegral = 0.0f;
rightIntegral = 0.0f;
leftPreviousError = 0.0f;
rightPreviousError = 0.0f;
previousState =
currentState;
}
// =====================================================
// IMMEDIATE HARD-STOP OUTPUT
// =====================================================
bool isHardStopState()
{
return
currentState == STATE_EMERGENCY_STOP
||
currentState == STATE_SYSTEM_OFF
||
currentState == STATE_CRITICAL_STOP
||
currentState == STATE_NO_SAFE_PATH
||
currentState == STATE_MOTOR_STALL;
}
void applyHardStopOutputs()
{
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
leftPWM = 0;
rightPWM = 0;
leftIntegral = 0.0f;
rightIntegral = 0.0f;
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
}
// =====================================================
// FAST RACE / SAFETY CONTROL
// =====================================================
//
// This is the fast steering loop.
//
// It reads ONLY the IR boundary array and updates the
// navigation target quickly. It does not wait for the
// slower RPM/PID cycle.
//
// This is important for a fast robot because waiting
// 250 ms before noticing a road edge would be far too
// slow at racing speed.
// =====================================================
void updateFastRoadControl()
{
unsigned long now =
millis();
if (
now - previousRoadControlTime
<
ROAD_CONTROL_INTERVAL_MS
)
{
return;
}
previousRoadControlTime =
now;
updateWokwiBridgeScenario();
updateVisionSerial();
updateBridgePhase();
// Internally rate-limited to 20 Hz.
updateToF();
readBoundarySensors();
analyseRoadBoundaries();
determineNavigationState();
enforceValidNavigationState();
handleStateChange();
applyNavigationTargets();
if (isHardStopState())
{
applyHardStopOutputs();
}
}
// =====================================================
// SPEED RAMP UPDATE
// =====================================================
float rampTowardTarget(
float currentValue,
float requestedValue,
float dt
)
{
if (dt <= 0.0f)
return currentValue;
float difference =
requestedValue -
currentValue;
if (fabsf(difference) < 0.001f)
return requestedValue;
// Speed increases use the gentle acceleration rate.
// Speed reductions use the faster deceleration rate.
float rate =
(
fabsf(requestedValue) >
fabsf(currentValue)
)
?
ACCEL_RAMP_RPM_PER_SEC
:
DECEL_RAMP_RPM_PER_SEC;
float maximumStep =
rate *
dt;
if (difference > maximumStep)
return currentValue + maximumStep;
if (difference < -maximumStep)
return currentValue - maximumStep;
return requestedValue;
}
void updateSpeedRamp(
float dt
)
{
// Safety stops NEVER use a ramp.
if (isHardStopState())
{
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
return;
}
leftRampedTargetRPM =
rampTowardTarget(
leftRampedTargetRPM,
leftTargetRPM,
dt
);
rightRampedTargetRPM =
rampTowardTarget(
rightRampedTargetRPM,
rightTargetRPM,
dt
);
}
// =====================================================
// RPM SAMPLE VALIDATION
// =====================================================
bool isValidRPMSample(float rpm)
{
return
isfinite(rpm)
&&
fabsf(rpm) <= MAX_VALID_RPM;
}
// =====================================================
// PID UPDATE
// =====================================================
void updatePID()
{
unsigned long now =
millis();
if (
now - previousPIDTime <
PID_INTERVAL_MS
)
{
return;
}
float dt =
(
now -
previousPIDTime
)
/
1000.0f;
previousPIDTime =
now;
// RPM
long leftDelta =
leftEncoderCount -
previousLeftPIDCount;
long rightDelta =
rightEncoderCount -
previousRightPIDCount;
previousLeftPIDCount =
leftEncoderCount;
previousRightPIDCount =
rightEncoderCount;
float newLeftRPM =
(
(float)leftDelta /
CPR
)
*
(
60.0f /
dt
);
float newRightRPM =
(
(float)rightDelta /
CPR
)
*
(
60.0f /
dt
);
// Reject NaN, infinity, or an impossible Wokwi RPM spike.
// If a bad sample occurs, keep the previous valid RPM so
// one corrupt encoder sample cannot destabilise the PID.
leftRPMValid =
isValidRPMSample(
newLeftRPM
);
rightRPMValid =
isValidRPMSample(
newRightRPM
);
if (leftRPMValid)
{
leftRawRPM =
newLeftRPM;
if (!leftRPMFilterInitialised)
{
leftRPM =
newLeftRPM;
leftRPMFilterInitialised =
true;
}
else
{
leftRPM =
RPM_FILTER_ALPHA *
newLeftRPM
+
(1.0f - RPM_FILTER_ALPHA) *
leftRPM;
}
}
if (rightRPMValid)
{
rightRawRPM =
newRightRPM;
if (!rightRPMFilterInitialised)
{
rightRPM =
newRightRPM;
rightRPMFilterInitialised =
true;
}
else
{
rightRPM =
RPM_FILTER_ALPHA *
newRightRPM
+
(1.0f - RPM_FILTER_ALPHA) *
rightRPM;
}
}
// SLOWER HEALTH / STABILITY PERCEPTION
//
// Camera, IR and ToF are handled by the fast race loop.
// This PID cycle updates current sensing and IMU data.
updateINA219();
imuValid =
readGyroZ();
if (!imuValid)
{
gyroZ = 0.0f;
pitchDeg = 0.0f;
}
updateBridgePhase();
updateSkidDetection();
updateMotorStallDetection();
// SAFETY / STABILITY DECISION
//
// Re-run the state selector after the slower health
// sensors update. Motor stall and skid states can then
// override normal road driving.
determineNavigationState();
enforceValidNavigationState();
handleStateChange();
applyNavigationTargets();
// Apply the physics-aware acceleration/deceleration ramp
// before the PID calculates motor error.
updateSpeedRamp(
dt
);
// HARD STOP
if (isHardStopState())
{
leftPWM = 0;
rightPWM = 0;
leftIntegral = 0.0f;
rightIntegral = 0.0f;
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
return;
}
// ===================================================
// POSITIONAL PID + FEED-FORWARD + ANTI-WINDUP
// ===================================================
//
// Previous controller:
// PWM = old PWM + correction
//
// That could accumulate output and overshoot.
//
// New controller:
// PWM = feed-forward + current PID correction
//
// The output is recalculated fresh every PID cycle.
// ===================================================
float leftError =
leftRampedTargetRPM -
leftRPM;
float rightError =
rightRampedTargetRPM -
rightRPM;
float leftDerivative =
(
leftError -
leftPreviousError
)
/
dt;
float rightDerivative =
(
rightError -
rightPreviousError
)
/
dt;
leftPreviousError =
leftError;
rightPreviousError =
rightError;
float leftCandidateIntegral =
constrain(
leftIntegral +
leftError * dt,
-PID_INTEGRAL_LIMIT,
PID_INTEGRAL_LIMIT
);
float rightCandidateIntegral =
constrain(
rightIntegral +
rightError * dt,
-PID_INTEGRAL_LIMIT,
PID_INTEGRAL_LIMIT
);
float leftFeedForward =
leftRampedTargetRPM *
PWM_FEEDFORWARD_PER_RPM;
float rightFeedForward =
rightRampedTargetRPM *
PWM_FEEDFORWARD_PER_RPM;
float leftCandidateOutput =
leftFeedForward
+
Kp * leftError
+
Ki * leftCandidateIntegral
+
Kd * leftDerivative;
float rightCandidateOutput =
rightFeedForward
+
Kp * rightError
+
Ki * rightCandidateIntegral
+
Kd * rightDerivative;
bool allowLeftIntegral =
(
(
leftCandidateOutput >= 0.0f
&&
leftCandidateOutput <= 255.0f
)
||
(
leftCandidateOutput > 255.0f
&&
leftError < 0.0f
)
||
(
leftCandidateOutput < 0.0f
&&
leftError > 0.0f
)
);
bool allowRightIntegral =
(
(
rightCandidateOutput >= 0.0f
&&
rightCandidateOutput <= 255.0f
)
||
(
rightCandidateOutput > 255.0f
&&
rightError < 0.0f
)
||
(
rightCandidateOutput < 0.0f
&&
rightError > 0.0f
)
);
if (allowLeftIntegral)
{
leftIntegral =
leftCandidateIntegral;
}
if (allowRightIntegral)
{
rightIntegral =
rightCandidateIntegral;
}
if (
leftRPM >
leftRampedTargetRPM +
OVERSPEED_MARGIN_RPM
&&
leftIntegral > 0.0f
)
{
leftIntegral *=
OVERSPEED_INTEGRAL_BLEED;
}
if (
rightRPM >
rightRampedTargetRPM +
OVERSPEED_MARGIN_RPM
&&
rightIntegral > 0.0f
)
{
rightIntegral *=
OVERSPEED_INTEGRAL_BLEED;
}
float leftPWMCommand =
leftFeedForward
+
Kp * leftError
+
Ki * leftIntegral
+
Kd * leftDerivative;
float rightPWMCommand =
rightFeedForward
+
Kp * rightError
+
Ki * rightIntegral
+
Kd * rightDerivative;
leftPWM =
constrain(
(int)roundf(
leftPWMCommand
),
0,
255
);
rightPWM =
constrain(
(int)roundf(
rightPWMCommand
),
0,
255
);
}
// =====================================================
// NAVIGATION STATE VALIDATION / FAIL-SAFE
// =====================================================
//
// currentState should only contain one of the enum values
// assigned by determineNavigationState().
//
// If an invalid value ever appears, fail SAFE:
// invalid state -> CRITICAL STOP -> PWM 0 / 0
// =====================================================
bool navigationStateIsValid()
{
switch (currentState)
{
case STATE_EMERGENCY_STOP:
case STATE_VISION_DRIVE:
case STATE_VISION_FALLBACK:
case STATE_ROAD_DRIVE:
case STATE_BOUNDARY_CORRECT_LEFT:
case STATE_BOUNDARY_CORRECT_RIGHT:
case STATE_BOUNDARY_CAUTION:
case STATE_BOUNDARY_ESCAPE:
case STATE_CAUTION:
case STATE_AVOID_LEFT:
case STATE_AVOID_RIGHT:
case STATE_CRITICAL_AVOID_LEFT:
case STATE_CRITICAL_AVOID_RIGHT:
case STATE_CRITICAL_STOP:
case STATE_NO_SAFE_PATH:
case STATE_MOTOR_STALL:
case STATE_SKID_CAUTION:
case STATE_SENSOR_FAULT:
case STATE_SYSTEM_OFF:
case STATE_BRIDGE_APPROACH:
case STATE_BRIDGE_CLIMB:
case STATE_BRIDGE_CREST:
case STATE_BRIDGE_DESCENT:
case STATE_BRIDGE_EXIT:
return true;
}
return false;
}
void enforceValidNavigationState()
{
if (navigationStateIsValid())
{
return;
}
invalidStateRecoveryCount++;
currentState =
STATE_CRITICAL_STOP;
leftTargetRPM = 0.0f;
rightTargetRPM = 0.0f;
leftRampedTargetRPM = 0.0f;
rightRampedTargetRPM = 0.0f;
leftPWM = 0;
rightPWM = 0;
leftIntegral = 0.0f;
rightIntegral = 0.0f;
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
}
// =====================================================
// STATE NAME
// =====================================================
const char *stateName()
{
switch (currentState)
{
case STATE_EMERGENCY_STOP:
return "EMERGENCY STOP";
case STATE_VISION_DRIVE:
return "VISION PREDICTIVE DRIVE";
case STATE_VISION_FALLBACK:
return "VISION FALLBACK - IR ONLY";
case STATE_ROAD_DRIVE:
return "ROAD DRIVE";
case STATE_BOUNDARY_CORRECT_LEFT:
return "BOUNDARY -> CORRECT LEFT";
case STATE_BOUNDARY_CORRECT_RIGHT:
return "BOUNDARY -> CORRECT RIGHT";
case STATE_BOUNDARY_CAUTION:
return "BOUNDARY CAUTION";
case STATE_BOUNDARY_ESCAPE:
return "BOUNDARY ESCAPE";
case STATE_CAUTION:
return "TOF CAUTION";
case STATE_AVOID_LEFT:
return "AVOID LEFT";
case STATE_AVOID_RIGHT:
return "AVOID RIGHT";
case STATE_CRITICAL_AVOID_LEFT:
return "CRITICAL AVOID LEFT";
case STATE_CRITICAL_AVOID_RIGHT:
return "CRITICAL AVOID RIGHT";
case STATE_CRITICAL_STOP:
return "CRITICAL STOP";
case STATE_NO_SAFE_PATH:
return "NO SAFE PATH - STOP";
case STATE_MOTOR_STALL:
return "MOTOR STALL";
case STATE_SKID_CAUTION:
return "POSSIBLE SKID";
case STATE_SENSOR_FAULT:
return "TOF SENSOR FAULT - DEGRADED";
case STATE_SYSTEM_OFF:
return "SYSTEM OFF";
case STATE_BRIDGE_APPROACH:
return "BRIDGE APPROACH";
case STATE_BRIDGE_CLIMB:
return "BRIDGE CLIMB";
case STATE_BRIDGE_CREST:
return "BRIDGE CREST";
case STATE_BRIDGE_DESCENT:
return "BRIDGE DESCENT";
case STATE_BRIDGE_EXIT:
return "BRIDGE EXIT";
}
return "INVALID STATE - FAILSAFE";
}
// =====================================================
// PRINT TTC
// =====================================================
void printTTCValue(float value)
{
if (value < 0.0f)
{
DebugSerial.print("N/A");
}
else
{
DebugSerial.print(value, 2);
DebugSerial.print(" s");
}
}
// =====================================================
// DIAGNOSTICS
// =====================================================
void printDiagnostics()
{
unsigned long now =
millis();
if (
now - previousPrintTime <
PRINT_INTERVAL_MS
)
{
return;
}
previousPrintTime = now;
DebugSerial.println();
DebugSerial.println(
"========================================"
);
DebugSerial.print(
"RUN MODE : "
);
DebugSerial.println(
RUNNING_IN_WOKWI
? "WOKWI SCALED RPM"
: (
PHYSICAL_INITIAL_TEST_PROFILE
? "PHYSICAL INITIAL TEST"
: "PHYSICAL RACE"
)
);
DebugSerial.print(
"NORMAL RPM LIMIT : "
);
DebugSerial.println(
NORMAL_BASE_RPM,
1
);
DebugSerial.print(
"STATE : "
);
DebugSerial.println(
stateName()
);
DebugSerial.print(
"STATE CODE : "
);
DebugSerial.println(
(int)currentState
);
DebugSerial.print(
"STATE FAILSAFE CNT: "
);
DebugSerial.println(
invalidStateRecoveryCount
);
DebugSerial.print(
"E-STOP STATUS : "
);
DebugSerial.println(
emergencyStopActive
? "PRESSED"
: "RELEASED"
);
DebugSerial.print(
"MAIN SWITCH : "
);
DebugSerial.println(
mainSwitchOn
? "ON"
: "OFF"
);
DebugSerial.print(
"START INTERLOCK : "
);
if (raceStartInterlockCleared)
{
DebugSerial.println(
"RACE ENABLED"
);
}
else if (!mainSwitchOn)
{
DebugSerial.println(
"READY - SWITCH ON TO START"
);
}
else
{
DebugSerial.println(
"LOCKED - SWITCH OFF FIRST"
);
}
DebugSerial.print(
"IR BOUNDARY : "
);
for (int i = 0; i < 8; i++)
{
DebugSerial.print(sensor[i]);
}
DebugSerial.println();
DebugSerial.print(
"ROAD STATUS : "
);
if (!boundaryDetected)
{
DebugSerial.println(
"CLEAR"
);
}
else if (
centreBoundaryDetected
&&
fabsf(boundarySteerError) <= 0.25f
)
{
DebugSerial.println(
"CENTRE BOUNDARY / ESCAPE"
);
}
else if (
boundarySteerError > 0.25f
)
{
DebugSerial.println(
"LEFT BOUNDARY - STEER RIGHT"
);
}
else if (
boundarySteerError < -0.25f
)
{
DebugSerial.println(
"RIGHT BOUNDARY - STEER LEFT"
);
}
else
{
DebugSerial.println(
"BOTH SIDES / BALANCED"
);
}
DebugSerial.print(
"VISION SOURCE : "
);
DebugSerial.println(
USE_WOKWI_INTERNAL_VISION_SIM
? "WOKWI INTERNAL SIM"
: "RASPBERRY PI UART"
);
DebugSerial.print(
"VISION STATUS : "
);
if (visionIsTrusted())
{
DebugSerial.println(
"ONLINE / TRUSTED"
);
}
else if (visionIsOnline())
{
DebugSerial.println(
"ONLINE / LOW CONFIDENCE"
);
}
else
{
DebugSerial.println(
"OFFLINE / STALE"
);
}
DebugSerial.print(
"VISION STEER : "
);
DebugSerial.print(
visionSteerPct
);
DebugSerial.println(
" %"
);
DebugSerial.print(
"VISION SPEED : "
);
DebugSerial.print(
visionSpeedPct
);
DebugSerial.println(
" %"
);
DebugSerial.print(
"VISION CONFIDENCE : "
);
DebugSerial.print(
visionConfidencePct
);
DebugSerial.println(
" %"
);
DebugSerial.print(
"BRIDGE CAM CONF : "
);
DebugSerial.print(
visionBridgeConfidencePct
);
DebugSerial.println(
" %"
);
DebugSerial.print(
"CORNER TEST : "
);
DebugSerial.println(
WOKWI_AUTO_CORNER_SPEED_TEST
? "ACTIVE / INDEPENDENT"
: "DISABLED"
);
DebugSerial.print(
"CORNER SEVERITY : "
);
DebugSerial.print(
visionCornerSeverityPct
);
DebugSerial.println(
" %"
);
DebugSerial.print(
"CORNER CLASS : "
);
DebugSerial.println(
cornerClassName()
);
DebugSerial.print(
"CORNER RPM CAP : "
);
DebugSerial.println(
getVisionCornerSpeedCapRPM(),
2
);
DebugSerial.print(
"VISION BASE RPM : "
);
DebugSerial.println(
getVisionBaseRPM(),
2
);
DebugSerial.print(
"PI RX BYTES : "
);
DebugSerial.println(
piRxByteCount
);
DebugSerial.print(
"PI GOOD PACKETS : "
);
DebugSerial.println(
piGoodPacketCount
);
DebugSerial.print(
"PI BAD PACKETS : "
);
DebugSerial.println(
piBadPacketCount
);
DebugSerial.print(
"PI LAST RAW : "
);
DebugSerial.println(
piLastRawPacket
);
DebugSerial.print(
"BOUNDARY STEER : "
);
DebugSerial.println(
boundarySteerError,
2
);
DebugSerial.print(
"LAST BOUNDARY : "
);
DebugSerial.println(
lastBoundarySteerError,
2
);
DebugSerial.print(
"BOUNDARY SEVERITY : "
);
DebugSerial.println(
boundarySeverity,
1
);
DebugSerial.println();
DebugSerial.print(
"TOF SOURCE : "
);
if (USE_WOKWI_INTERNAL_TOF_SIM)
{
DebugSerial.println(
"WOKWI INTERNAL"
);
}
else if (USE_WOKWI_TOF_SLIDER_CHIPS)
{
DebugSerial.println(
"VL53L1X SLIDERS"
);
}
else
{
DebugSerial.println(
"PHYSICAL I2C"
);
}
DebugSerial.print(
"IMU SOURCE : "
);
DebugSerial.println(
USE_WOKWI_INTERNAL_IMU_SIM
? "WOKWI INTERNAL"
: "PHYSICAL I2C"
);
DebugSerial.print(
"INA SOURCE : "
);
if (USE_WOKWI_INTERNAL_INA_SIM)
{
DebugSerial.println(
"WOKWI INTERNAL"
);
}
else if (USE_WOKWI_INA219_SLIDER_CHIP)
{
DebugSerial.println(
"INA219 SLIDERS"
);
}
else
{
DebugSerial.println(
"PHYSICAL I2C"
);
}
DebugSerial.print(
"LEFT TOF : "
);
if (tofValid)
{
DebugSerial.print(leftDistance);
DebugSerial.println(" mm");
}
else
{
DebugSerial.println("INVALID");
}
DebugSerial.print(
"RIGHT TOF : "
);
if (tofValid)
{
DebugSerial.print(rightDistance);
DebugSerial.println(" mm");
}
else
{
DebugSerial.println("INVALID");
}
DebugSerial.print(
"LEFT CLOSING : "
);
DebugSerial.print(leftClosingSpeed, 1);
DebugSerial.println(" mm/s");
DebugSerial.print(
"RIGHT CLOSING : "
);
DebugSerial.print(rightClosingSpeed, 1);
DebugSerial.println(" mm/s");
DebugSerial.print(
"LEFT TTC : "
);
printTTCValue(leftTTC);
DebugSerial.println();
DebugSerial.print(
"RIGHT TTC : "
);
printTTCValue(rightTTC);
DebugSerial.println();
DebugSerial.println();
DebugSerial.print(
"TRACK MODE : "
);
DebugSerial.println(
"CAMERA + IR + TOF RACING"
);
DebugSerial.print(
"OBSTACLE MODE : "
);
DebugSerial.println(
OBSTACLE_AVOIDANCE_ENABLED
? "ENABLED"
: "ENABLED - CARS / WALLS"
);
DebugSerial.print(
"ROAD LOOP : "
);
DebugSerial.print(
1000UL /
ROAD_CONTROL_INTERVAL_MS
);
DebugSerial.println(
" Hz"
);
DebugSerial.println();
DebugSerial.print(
"INA219 STATUS : "
);
DebugSerial.println(
inaValid
? "ONLINE"
: "READ ERROR"
);
DebugSerial.print(
"BUS VOLTAGE : "
);
if (inaValid)
{
DebugSerial.print(
busVoltageV,
2
);
DebugSerial.println(
" V"
);
}
else
{
DebugSerial.println(
"N/A"
);
}
DebugSerial.print(
"MOTOR CURRENT : "
);
if (inaValid)
{
DebugSerial.print(
motorCurrentMA,
0
);
DebugSerial.println(
" mA"
);
}
else
{
DebugSerial.println(
"N/A"
);
}
DebugSerial.print(
"CURRENT STATUS : "
);
DebugSerial.println(
currentStatusName()
);
DebugSerial.print(
"STALL FLAG : "
);
DebugSerial.println(
motorStallDetected
? "YES"
: "NO"
);
DebugSerial.print(
"STALL LATCH : "
);
DebugSerial.println(
motorStallLatched
? "LOCKED"
: "CLEAR"
);
DebugSerial.print(
"STALL SIDE : "
);
DebugSerial.println(
stallSideName()
);
DebugSerial.print(
"STALL RESET : "
);
DebugSerial.println(
motorStallLatched
? "MAIN SWITCH OFF -> ON REQUIRED"
: "READY"
);
DebugSerial.println();
DebugSerial.print(
"MPU STATUS : "
);
DebugSerial.println(
imuValid
? "ONLINE"
: "READ ERROR"
);
DebugSerial.print(
"GYRO Z / YAW : "
);
DebugSerial.print(
gyroZ,
2
);
DebugSerial.println(
" deg/s"
);
DebugSerial.print(
"PITCH : "
);
DebugSerial.print(
pitchDeg,
2
);
DebugSerial.println(
" deg"
);
DebugSerial.print(
"BRIDGE PHASE : "
);
DebugSerial.println(
bridgePhaseName()
);
DebugSerial.print(
"SKID FLAG : "
);
DebugSerial.println(
possibleSkid
? "YES"
: "NO"
);
DebugSerial.println();
DebugSerial.print(
"LEFT REQUEST RPM : "
);
DebugSerial.println(
leftTargetRPM,
2
);
DebugSerial.print(
"RIGHT REQUEST RPM : "
);
DebugSerial.println(
rightTargetRPM,
2
);
DebugSerial.print(
"LEFT PID SETPOINT : "
);
DebugSerial.println(
leftRampedTargetRPM,
2
);
DebugSerial.print(
"RIGHT PID SETPOINT: "
);
DebugSerial.println(
rightRampedTargetRPM,
2
);
DebugSerial.print(
"LEFT ACTUAL RPM : "
);
if (leftRPMValid)
{
DebugSerial.println(
leftRPM,
2
);
}
else
{
DebugSerial.println(
"INVALID"
);
}
DebugSerial.print(
"RIGHT ACTUAL RPM : "
);
if (rightRPMValid)
{
DebugSerial.println(
rightRPM,
2
);
}
else
{
DebugSerial.println(
"INVALID"
);
}
DebugSerial.print(
"LEFT RAW RPM : "
);
DebugSerial.println(
leftRawRPM,
2
);
DebugSerial.print(
"RIGHT RAW RPM : "
);
DebugSerial.println(
rightRawRPM,
2
);
DebugSerial.print(
"RPM SAMPLE STATUS : "
);
DebugSerial.println(
(leftRPMValid && rightRPMValid)
? "OK"
: "CHECK ENCODERS"
);
DebugSerial.println(
"PID MODE : POSITIONAL + FF + ANTI-WINDUP"
);
DebugSerial.print(
"LEFT PWM : "
);
DebugSerial.println(leftPWM);
DebugSerial.print(
"RIGHT PWM : "
);
DebugSerial.println(rightPWM);
// Ensure the complete diagnostics frame leaves the UART buffer
DebugSerial.flush();
}
// =====================================================
// SETUP
// =====================================================
void setup()
{
DebugSerial.begin(115200);
PiSerial.begin(115200);
// Allow the first ToF update during setup.
previousToFTime =
millis() -
TOF_INTERVAL_MS;
delay(500);
// IMPORTANT:
// Print immediately so we know setup has started.
DebugSerial.println();
DebugSerial.println(
"========================================"
);
DebugSerial.println(
" ROBO RUMBLE - FULL SENSOR RACE CONTROLLER"
);
DebugSerial.println(
"========================================"
);
DebugSerial.println();
DebugSerial.println(
"[BOOT] Setup started"
);
DebugSerial.println(
"[BOOT] Pi vision UART: PB11 RX / PB10 TX @ 115200"
);
DebugSerial.print(
"[BOOT] Vision source: "
);
DebugSerial.println(
USE_WOKWI_INTERNAL_VISION_SIM
? "WOKWI INTERNAL SIM"
: "RASPBERRY PI UART"
);
DebugSerial.println(
"[BOOT] Pi RX diagnostics enabled"
);
DebugSerial.println(
"[BOOT] FINAL INTEGRATED MODE: scripted tests DISABLED"
);
DebugSerial.println(
"[BOOT] STARTUP INTERLOCK: MAIN must be OFF, then switched ON"
);
DebugSerial.println(
"[BOOT] MAIN SWITCH: 60 ms DEBOUNCE + ONE-SHOT RACE START"
);
DebugSerial.println(
"[BOOT] Sensor sources:"
);
DebugSerial.print(
" ToF : "
);
if (USE_WOKWI_INTERNAL_TOF_SIM)
{
DebugSerial.println(
"WOKWI INTERNAL"
);
}
else if (USE_WOKWI_TOF_SLIDER_CHIPS)
{
DebugSerial.println(
"VL53L1X SLIDERS"
);
}
else
{
DebugSerial.println(
"PHYSICAL I2C"
);
}
DebugSerial.print(
" IMU : "
);
DebugSerial.println(
USE_WOKWI_INTERNAL_IMU_SIM
? "WOKWI INTERNAL"
: "PHYSICAL I2C"
);
DebugSerial.print(
" INA : "
);
DebugSerial.println(
USE_WOKWI_INTERNAL_INA_SIM
? "WOKWI INTERNAL"
: "PHYSICAL I2C"
);
// EMERGENCY STOP
pinMode(
ESTOP_PIN,
INPUT_PULLUP
);
readEmergencyStop();
// MOTOR PINS
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(STBY, OUTPUT);
// SAFE BOOT OUTPUTS:
// Before any sensor initialisation or race logic, both PWM
// outputs and the motor driver are forced OFF.
digitalWrite(PWMA, LOW);
digitalWrite(PWMB, LOW);
digitalWrite(STBY, LOW);
// ENCODERS
pinMode(LEFT_ENC_A, INPUT);
pinMode(LEFT_ENC_B, INPUT);
pinMode(RIGHT_ENC_A, INPUT);
pinMode(RIGHT_ENC_B, INPUT);
// IR
for (int i = 0; i < 8; i++)
{
pinMode(
IR_PINS[i],
INPUT
);
}
// TOF XSHUT
//
// PB3/PB4 are used as GPIO for the two ToF shutdown pins.
// On physical STM32F103 hardware these pins overlap with
// the default JTAG interface. If supported by the core,
// disable JTAG while retaining SWD.
//
// For this Wokwi model the XSHUT lines stay HIGH because
// the two simulated sensors already have unique addresses.
// The race-safe custom ToF chip deliberately ignores live
// XSHUT disconnects to avoid a Wokwi custom-I2C bus lockup.
#ifdef __HAL_AFIO_REMAP_SWJ_NOJTAG
__HAL_RCC_AFIO_CLK_ENABLE();
__HAL_AFIO_REMAP_SWJ_NOJTAG();
#endif
// MAIN ON/OFF SWITCH
// PA15 is usable as GPIO after JTAG is disabled.
pinMode(
MAIN_SWITCH_PIN,
INPUT_PULLUP
);
// Initialise the debounced MAIN-switch state.
// This NEVER starts the race at boot, even if the switch is ON.
initialiseMainSwitch();
pinMode(
LEFT_XSHUT,
OUTPUT
);
pinMode(
RIGHT_XSHUT,
OUTPUT
);
// ===================================================
// WOKWI-SAFE XSHUT INITIALISATION
// ===================================================
//
// IMPORTANT:
// Our two custom Wokwi VL53L1X chips already have fixed
// simulation addresses (0x30 and 0x31).
//
// Toggling a custom I2C device off and back on with
// XSHUT can make Wokwi's shared custom-I2C bus hang.
//
// Therefore, in Wokwi we simply keep both sensors
// enabled from startup.
//
// The PHYSICAL robot is different: real VL53L1X
// sensors normally start at the same default address,
// so physical firmware will later sequence XSHUT and
// assign unique I2C addresses during startup.
// ===================================================
digitalWrite(
LEFT_XSHUT,
HIGH
);
digitalWrite(
RIGHT_XSHUT,
HIGH
);
DebugSerial.println(
"[BOOT] GPIO configured"
);
// I2C
//
// The bus remains configured even when the integrated
// Wokwi internal-sensor mode is enabled. This preserves
// the physical pin map in one codebase.
Wire.setSDA(PB7);
Wire.setSCL(PB6);
Wire.begin();
Wire.setClock(
100000
);
DebugSerial.println(
"[BOOT] I2C started"
);
delay(100);
// TOF
//
// Do not perform a VL53L1X transaction inside setup().
// The custom Wokwi I2C pair is serviced by the normal
// race loop after all devices have finished booting.
//
// This also keeps startup deterministic: the Serial
// Monitor reaches READY before the first ToF sample.
DebugSerial.println(
"[BOOT] VL53L1X pair: DEFERRED TO RACE LOOP"
);
// MPU
DebugSerial.println(
"[BOOT] Initialising MPU6050..."
);
imuValid =
initialiseMPU();
DebugSerial.print(
"[BOOT] MPU6050: "
);
DebugSerial.println(
imuValid
? "ONLINE"
: "FAILED"
);
// INA219
DebugSerial.println(
"[BOOT] Checking INA219..."
);
updateINA219(
true
);
DebugSerial.print(
"[BOOT] INA219: "
);
DebugSerial.println(
inaValid
? "ONLINE"
: "FAILED"
);
// ENCODER INITIAL STATES
previousLeftEncoderState =
(
digitalRead(LEFT_ENC_A)
<< 1
)
|
digitalRead(LEFT_ENC_B);
previousRightEncoderState =
(
digitalRead(RIGHT_ENC_A)
<< 1
)
|
digitalRead(RIGHT_ENC_B);
DebugSerial.println(
"[BOOT] Encoders configured"
);
// MOTORS
motorsForward();
if (emergencyStopActive)
{
applyEmergencyStopImmediately();
DebugSerial.println(
"[BOOT] E-STOP: PRESSED - MOTOR DRIVER DISABLED"
);
}
else
{
DebugSerial.println(
"[BOOT] E-STOP: RELEASED"
);
}
DebugSerial.print(
"[BOOT] MAIN SWITCH: "
);
DebugSerial.println(
mainSwitchOn
? "ON"
: "OFF"
);
pwmPeriodStart =
micros();
previousPIDTime =
millis();
previousRoadControlTime =
millis();
previousToFTime =
millis() -
TOF_INTERVAL_MS;
previousINATime =
millis() -
INA_INTERVAL_MS;
DebugSerial.println(
"[BOOT] Motor driver enabled"
);
DebugSerial.print(
"[BOOT] Speed ramp: ACCEL "
);
DebugSerial.print(
ACCEL_RAMP_RPM_PER_SEC,
1
);
DebugSerial.print(
" RPM/s | DECEL "
);
DebugSerial.print(
DECEL_RAMP_RPM_PER_SEC,
1
);
DebugSerial.println(
" RPM/s"
);
DebugSerial.println(
"[BOOT] PID: POSITIONAL + FEED-FORWARD + ANTI-WINDUP"
);
DebugSerial.println(
"[BOOT] Stall latch: MANUAL MAIN OFF -> ON RESET"
);
DebugSerial.println(
RUNNING_IN_WOKWI
? "[BOOT] INA219 source: INTERACTIVE WOKWI SLIDERS"
: "[BOOT] INA219 source: PHYSICAL I2C"
);
DebugSerial.print(
"[BOOT] Run profile: "
);
DebugSerial.println(
RUNNING_IN_WOKWI
? "WOKWI SCALED RPM"
: (
PHYSICAL_INITIAL_TEST_PROFILE
? "PHYSICAL INITIAL TEST"
: "PHYSICAL RACE"
)
);
DebugSerial.print(
"[BOOT] Normal target RPM: "
);
DebugSerial.println(
NORMAL_BASE_RPM,
1
);
DebugSerial.print(
"[BOOT] Dynamic corner-speed test: "
);
DebugSerial.println(
WOKWI_AUTO_CORNER_SPEED_TEST
? "ENABLED"
: "DISABLED"
);
DebugSerial.println(
"[BOOT] Wokwi test scenario: CORNER SPEED ONLY"
);
DebugSerial.print(
"[BOOT] Autonomous bridge test: "
);
DebugSerial.println(
WOKWI_AUTO_BRIDGE_TEST
? "ENABLED"
: "DISABLED"
);
DebugSerial.println(
"[BOOT] Bridge profile: MILD SLOPE / HIGHER SPEED"
);
DebugSerial.println();
DebugSerial.println(
"[READY] Starting autonomous control..."
);
DebugSerial.println();
}
// =====================================================
// LOOP
// =====================================================
void loop()
{
// ===================================================
// EMERGENCY STOP IS CHECKED EVERY LOOP
// ===================================================
readEmergencyStop();
readMainSwitch();
if (emergencyStopActive)
{
applyEmergencyStopImmediately();
// Keep sampling encoders so diagnostics can show
// the wheels winding down after the drive is cut.
readLeftEncoder();
readRightEncoder();
printDiagnostics();
return;
}
if (
!mainSwitchOn
||
!raceStartInterlockCleared
)
{
applySystemOffImmediately();
// In Wokwi the MCU stays alive so we can display
// SYSTEM OFF diagnostics.
readLeftEncoder();
readRightEncoder();
printDiagnostics();
return;
}
// E-Stop released + main switch ON:
digitalWrite(
STBY,
HIGH
);
serviceMotors();
updateWokwiBridgeScenario();
// Read Raspberry Pi look-ahead commands as soon as possible.
updateVisionSerial();
// Fast steering / road-edge reaction
updateFastRoadControl();
// Slower wheel-speed PID + health/stability checks
updatePID();
printDiagnostics();
}