// =====================================================
// SMART SHOPPING TROLLEY - AUTONOMOUS FOLLOWING SYSTEM
// With Safe Turning Logic & Collision Prevention
// =====================================================
//
// ╔═══════════════════════════════════════════════════════════════════════════╗
// ║ PIN CONNECTION GUIDE ║
// ╠═══════════════════════════════════════════════════════════════════════════╣
// ║ ║
// ║ ESP32 DevKit V1 / WROOM-32 Pin Configuration ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ ULTRASONIC SENSORS (HC-SR04) - 4 Units │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ FRONT SENSOR: │ ║
// ║ │ VCC → 5V │ ║
// ║ │ GND → GND │ ║
// ║ │ TRIG → GPIO 5 │ ║
// ║ │ ECHO → GPIO 18 (via voltage divider: 1kΩ + 2kΩ to GND) │ ║
// ║ │ │ ║
// ║ │ LEFT SENSOR: │ ║
// ║ │ VCC → 5V │ ║
// ║ │ GND → GND │ ║
// ║ │ TRIG → GPIO 17 │ ║
// ║ │ ECHO → GPIO 19 (via voltage divider) │ ║
// ║ │ │ ║
// ║ │ RIGHT SENSOR: │ ║
// ║ │ VCC → 5V │ ║
// ║ │ GND → GND │ ║
// ║ │ TRIG → GPIO 16 │ ║
// ║ │ ECHO → GPIO 36 (VP - Input only) │ ║
// ║ │ │ ║
// ║ │ BACK SENSOR: │ ║
// ║ │ VCC → 5V │ ║
// ║ │ GND → GND │ ║
// ║ │ TRIG → GPIO 4 │ ║
// ║ │ ECHO → GPIO 39 (VN - Input only) │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ L298N MOTOR DRIVER │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ LEFT MOTOR (Motor A): │ ║
// ║ │ IN1 (Forward) → GPIO 32 │ ║
// ║ │ IN2 (Backward) → GPIO 33 │ ║
// ║ │ ENA (PWM Speed)→ GPIO 23 │ ║
// ║ │ OUT1, OUT2 → Left Motor Terminals │ ║
// ║ │ │ ║
// ║ │ RIGHT MOTOR (Motor B): │ ║
// ║ │ IN3 (Forward) → GPIO 12 │ ║
// ║ │ IN4 (Backward) → GPIO 13 │ ║
// ║ │ ENB (PWM Speed)→ GPIO 15 │ ║
// ║ │ OUT3, OUT4 → Right Motor Terminals │ ║
// ║ │ │ ║
// ║ │ POWER: │ ║
// ║ │ +12V → Battery Positive (7-12V) │ ║
// ║ │ GND → Battery Negative + ESP32 GND (Common Ground!) │ ║
// ║ │ +5V → Can power ESP32 (if jumper enabled) │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ UWB MODULE (DW1000 / DWM1000) - I2C Mode │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ VCC → 3.3V (NOT 5V!) │ ║
// ║ │ GND → GND │ ║
// ║ │ SDA → GPIO 21 │ ║
// ║ │ SCL → GPIO 22 │ ║
// ║ │ IRQ → GPIO 34 (Optional - Input only pin) │ ║
// ║ │ RST → GPIO 27 (Optional - for reset control) │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ ToF SENSOR (GY-53 / VL53L0X) - UART Mode │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ VCC → 3.3V or 5V (check module) │ ║
// ║ │ GND → GND │ ║
// ║ │ TX → GPIO 35 (RX2 - Input only) │ ║
// ║ │ RX → GPIO 25 (TX2) - if bidirectional needed │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ LED INDICATORS │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ LED_FAST (Green) → GPIO 26 → 220Ω → LED → GND │ ║
// ║ │ LED_MED (Yellow) → GPIO 0 → 220Ω → LED → GND │ ║
// ║ │ LED_SLOW (Orange) → GPIO 2 → 220Ω → LED → GND │ ║
// ║ │ LED_STOP (Red) → GPIO 14 → 220Ω → LED → GND │ ║
// ║ │ │ ║
// ║ │ NOTE: GPIO 0 and 2 affect boot - ensure LEDs don't pull low at boot │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ IMPORTANT NOTES │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ • ESP32 GPIO is 3.3V tolerant - use voltage dividers for 5V Echo │ ║
// ║ │ • GPIO 34, 35, 36, 39 are INPUT ONLY (no internal pullup) │ ║
// ║ │ • GPIO 6-11 are used for flash - DO NOT USE │ ║
// ║ │ • Common GND between ESP32, motors, and all sensors is CRITICAL │ ║
// ║ │ • Use decoupling capacitors (100µF) near motor driver │ ║
// ║ │ • Keep motor wires away from sensor wires to reduce noise │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ║ ║
// ║ ┌─────────────────────────────────────────────────────────────────────┐ ║
// ║ │ VOLTAGE DIVIDER FOR HC-SR04 ECHO (5V → 3.3V) │ ║
// ║ ├─────────────────────────────────────────────────────────────────────┤ ║
// ║ │ │ ║
// ║ │ ECHO (5V) ──┬── 1kΩ ──┬── ESP32 GPIO │ ║
// ║ │ │ │ │ ║
// ║ │ GND 2kΩ │ ║
// ║ │ │ │ ║
// ║ │ GND │ ║
// ║ │ │ ║
// ║ │ Output = 5V × (2kΩ / 3kΩ) = 3.3V ✓ │ ║
// ║ └─────────────────────────────────────────────────────────────────────┘ ║
// ╚═══════════════════════════════════════════════════════════════════════════╝
// --- Ultrasonic Sensor Pins ---
#define TF 5 // Front Trigger
#define EF 18 // Front Echo
#define TL 17 // Left Trigger
#define EL 19 // Left Echo
#define TR 16 // Right Trigger
#define ER 36 // Right Echo (GPIO 36/VP - Input only, no conflict with I2C)
#define TB 4 // Back Trigger
#define EB 39 // Back Echo (GPIO 39/VN - Input only, no conflict with I2C)
// --- LED Indicators ---
#define LED_FAST 26 // Green - Fast speed
#define LED_MED 0 // Yellow - Medium speed (caution: boot pin)
#define LED_SLOW 2 // Orange - Slow speed (built-in LED on some boards)
#define LED_STOP 14 // Red - Stopped
// --- Motor Control Pins (L298N) ---
#define MOTOR_LEFT_FWD 32 // IN1
#define MOTOR_LEFT_BWD 33 // IN2
#define MOTOR_RIGHT_FWD 12 // IN3
#define MOTOR_RIGHT_BWD 13 // IN4
#define MOTOR_LEFT_EN 23 // ENA (PWM)
#define MOTOR_RIGHT_EN 15 // ENB (PWM) - Changed from GPIO 2 to avoid conflict
// --- UWB Module Pins (DW1000 - I2C) ---
#define UWB_SDA 21 // I2C Data
#define UWB_SCL 22 // I2C Clock
#define UWB_IRQ 34 // Interrupt (optional)
#define UWB_RST 27 // Reset (optional)
// --- ToF Sensor Pin (GY-53 - UART) ---
#define TOF_RX 35 // UART RX (receive from sensor)
#define TOF_TX 25 // UART TX (send to sensor, if needed)
// =====================================================
// SAFETY THRESHOLDS (in cm)
// =====================================================
#define SHELF_THRESHOLD 25 // Too close to shelf - danger zone
#define SAFE_TURN_DISTANCE 45 // Minimum clearance to initiate turn
#define FRONT_STOP_DISTANCE 30 // Emergency stop distance
#define FRONT_SLOW_DISTANCE 50 // Slow down distance
#define FRONT_MED_DISTANCE 80 // Medium speed distance
#define REAR_DANGER_DISTANCE 20 // Rear collision warning
#define USER_TOO_CLOSE 40 // ToF: too close to user
#define USER_TOO_FAR 150 // ToF: user too far
#define USER_LOST_DISTANCE 300 // User no longer detected
// --- UWB Angle Thresholds ---
#define TURN_ANGLE_THRESHOLD 30 // Degrees to trigger turn intent
#define HYSTERESIS_ANGLE 5 // Prevent flickering at threshold
// =====================================================
// MOTOR SPEED SETTINGS (PWM 0-255)
// =====================================================
#define SPEED_FAST 200
#define SPEED_MEDIUM 150
#define SPEED_SLOW 100
#define SPEED_CREEP 70 // For waiting to turn
#define SPEED_TURN 120 // During turns
// =====================================================
// STATE MACHINE
// =====================================================
enum TrolleyState {
STATE_IDLE,
STATE_FOLLOWING,
STATE_WAITING_TO_TURN_LEFT,
STATE_WAITING_TO_TURN_RIGHT,
STATE_TURNING_LEFT,
STATE_TURNING_RIGHT,
STATE_EMERGENCY_STOP,
STATE_BLOCKED,
STATE_USER_LOST, // Added: User not detected
STATE_REAR_DANGER // Added: Something approaching from behind
};
enum DesiredDirection {
DIR_NONE,
DIR_FORWARD,
DIR_LEFT,
DIR_RIGHT
};
// =====================================================
// GLOBAL VARIABLES
// =====================================================
TrolleyState currentState = STATE_IDLE;
TrolleyState previousState = STATE_IDLE; // Added: For state transition tracking
DesiredDirection userDirection = DIR_NONE;
DesiredDirection lastValidDirection = DIR_FORWARD; // Added: Remember last direction
float uwbAngle = 0.0;
float uwbDistance = 0.0;
long tofDistance = 0;
unsigned long turnWaitStartTime = 0;
unsigned long lastStateChangeTime = 0; // Added: Debounce state changes
unsigned long userLostTime = 0; // Added: Track when user was lost
const unsigned long MAX_TURN_WAIT = 5000; // Max 5 sec waiting
const unsigned long STATE_DEBOUNCE = 100; // Added: 100ms debounce
const unsigned long USER_LOST_TIMEOUT = 3000; // Added: 3 sec before stopping
// Sensor readings
long distFront, distLeft, distRight, distBack;
// Sensor validity flags (for error detection)
bool sensorFrontValid = true;
bool sensorLeftValid = true;
bool sensorRightValid = true;
bool sensorBackValid = true;
bool uwbValid = false; // Added: Track if UWB is connected
// =====================================================
// SENSOR FUNCTIONS
// =====================================================
long getDistance(int trig, int echo) {
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(10);
digitalWrite(trig, LOW);
long duration = pulseIn(echo, HIGH, 30000); // 30ms timeout
if (duration == 0) {
return 999; // No echo = far away or sensor error
}
long distance = duration * 0.034 / 2;
// Sanity check - HC-SR04 max range is ~400cm
if (distance > 400) {
return 400;
}
return distance;
}
void readAllSensors() {
// Read with small delays between to avoid interference
distFront = getDistance(TF, EF);
delayMicroseconds(50);
distLeft = getDistance(TL, EL);
delayMicroseconds(50);
distRight = getDistance(TR, ER);
delayMicroseconds(50);
distBack = getDistance(TB, EB);
// Validate sensor readings (detect disconnected sensors)
sensorFrontValid = (distFront < 998);
sensorLeftValid = (distLeft < 998);
sensorRightValid = (distRight < 998);
sensorBackValid = (distBack < 998);
// If sensor invalid, assume danger (fail-safe)
if (!sensorFrontValid) distFront = 0;
if (!sensorLeftValid) distLeft = 0;
if (!sensorRightValid) distRight = 0;
if (!sensorBackValid) distBack = 0;
}
// Simulated UWB reading - Replace with actual DW1000 library
void readUWB() {
// ─────────────────────────────────────────────────────────
// REAL IMPLEMENTATION EXAMPLE (uncomment and modify):
// ─────────────────────────────────────────────────────────
// #include <DW1000.h>
// DW1000Ranging.loop();
// if (DW1000Ranging.getDistantDevice()) {
// uwbDistance = DW1000Ranging.getDistantDevice()->getRange();
// uwbAngle = DW1000Ranging.getDistantDevice()->getAngle();
// uwbValid = true;
// } else {
// uwbValid = false;
// }
// ─────────────────────────────────────────────────────────
// SIMULATION MODE: Read angle from Serial for testing
// Send values like "L45" (left 45°) or "R30" (right 30°) or "F" (forward)
if (Serial.available() > 0) {
char cmd = Serial.read();
if (cmd == 'L' || cmd == 'l') {
int angle = Serial.parseInt();
uwbAngle = -abs(angle); // Negative = left
uwbValid = true;
} else if (cmd == 'R' || cmd == 'r') {
int angle = Serial.parseInt();
uwbAngle = abs(angle); // Positive = right
uwbValid = true;
} else if (cmd == 'F' || cmd == 'f') {
uwbAngle = 0;
uwbValid = true;
} else if (cmd == 'X' || cmd == 'x') {
uwbValid = false; // Simulate user lost
}
}
// Default simulation values (remove in production)
if (!uwbValid) {
uwbAngle = 0;
uwbDistance = 100;
}
}
// Simulated ToF reading - Replace with GY-53 library
void readToF() {
// ─────────────────────────────────────────────────────────
// REAL IMPLEMENTATION EXAMPLE for GY-53 (UART mode):
// ─────────────────────────────────────────────────────────
// HardwareSerial ToFSerial(2); // Use UART2
// ToFSerial.begin(9600, SERIAL_8N1, TOF_RX, TOF_TX);
//
// if (ToFSerial.available() >= 4) {
// if (ToFSerial.read() == 0x5A) { // Frame header
// byte high = ToFSerial.read();
// byte low = ToFSerial.read();
// tofDistance = (high << 8) | low;
// }
// }
// ─────────────────────────────────────────────────────────
// Use front ultrasonic as fallback when ToF not implemented
tofDistance = distFront;
}
// =====================================================
// MOTOR CONTROL FUNCTIONS
// =====================================================
void setupMotors() {
pinMode(MOTOR_LEFT_FWD, OUTPUT);
pinMode(MOTOR_LEFT_BWD, OUTPUT);
pinMode(MOTOR_RIGHT_FWD, OUTPUT);
pinMode(MOTOR_RIGHT_BWD, OUTPUT);
pinMode(MOTOR_LEFT_EN, OUTPUT);
pinMode(MOTOR_RIGHT_EN, OUTPUT);
stopMotors();
}
void stopMotors() {
digitalWrite(MOTOR_LEFT_FWD, LOW);
digitalWrite(MOTOR_LEFT_BWD, LOW);
digitalWrite(MOTOR_RIGHT_FWD, LOW);
digitalWrite(MOTOR_RIGHT_BWD, LOW);
analogWrite(MOTOR_LEFT_EN, 0);
analogWrite(MOTOR_RIGHT_EN, 0);
}
void moveForward(int speed) {
digitalWrite(MOTOR_LEFT_FWD, HIGH);
digitalWrite(MOTOR_LEFT_BWD, LOW);
digitalWrite(MOTOR_RIGHT_FWD, HIGH);
digitalWrite(MOTOR_RIGHT_BWD, LOW);
analogWrite(MOTOR_LEFT_EN, speed);
analogWrite(MOTOR_RIGHT_EN, speed);
}
void moveBackward(int speed) {
digitalWrite(MOTOR_LEFT_FWD, LOW);
digitalWrite(MOTOR_LEFT_BWD, HIGH);
digitalWrite(MOTOR_RIGHT_FWD, LOW);
digitalWrite(MOTOR_RIGHT_BWD, HIGH);
analogWrite(MOTOR_LEFT_EN, speed);
analogWrite(MOTOR_RIGHT_EN, speed);
}
// Smooth turn - differential speed (no pivot turns near shelves)
void turnLeft(int baseSpeed) {
digitalWrite(MOTOR_LEFT_FWD, HIGH);
digitalWrite(MOTOR_LEFT_BWD, LOW);
digitalWrite(MOTOR_RIGHT_FWD, HIGH);
digitalWrite(MOTOR_RIGHT_BWD, LOW);
analogWrite(MOTOR_LEFT_EN, (int)(baseSpeed * 0.4)); // Inner wheel slower (cast to int)
analogWrite(MOTOR_RIGHT_EN, baseSpeed); // Outer wheel faster
}
void turnRight(int baseSpeed) {
digitalWrite(MOTOR_LEFT_FWD, HIGH);
digitalWrite(MOTOR_LEFT_BWD, LOW);
digitalWrite(MOTOR_RIGHT_FWD, HIGH);
digitalWrite(MOTOR_RIGHT_BWD, LOW);
analogWrite(MOTOR_LEFT_EN, baseSpeed); // Outer wheel faster
analogWrite(MOTOR_RIGHT_EN, (int)(baseSpeed * 0.4)); // Inner wheel slower (cast to int)
}
// Slow creep forward while waiting to turn
void creepForward() {
moveForward(SPEED_CREEP);
}
// =====================================================
// LED INDICATOR FUNCTIONS
// =====================================================
void updateLEDs(TrolleyState state) {
digitalWrite(LED_FAST, LOW);
digitalWrite(LED_MED, LOW);
digitalWrite(LED_SLOW, LOW);
digitalWrite(LED_STOP, LOW);
switch(state) {
case STATE_FOLLOWING:
if (distFront > FRONT_MED_DISTANCE) {
digitalWrite(LED_FAST, HIGH);
} else if (distFront > FRONT_SLOW_DISTANCE) {
digitalWrite(LED_MED, HIGH);
} else {
digitalWrite(LED_SLOW, HIGH);
}
break;
case STATE_WAITING_TO_TURN_LEFT:
case STATE_WAITING_TO_TURN_RIGHT:
// Blink slow LED while waiting
digitalWrite(LED_SLOW, (millis() / 250) % 2);
break;
case STATE_TURNING_LEFT:
case STATE_TURNING_RIGHT:
digitalWrite(LED_MED, HIGH);
break;
case STATE_USER_LOST:
// Blink all LEDs when user lost
if ((millis() / 500) % 2) {
digitalWrite(LED_SLOW, HIGH);
digitalWrite(LED_MED, HIGH);
}
break;
case STATE_REAR_DANGER:
// Fast blink stop LED
digitalWrite(LED_STOP, (millis() / 100) % 2);
break;
case STATE_EMERGENCY_STOP:
case STATE_BLOCKED:
case STATE_IDLE:
default:
digitalWrite(LED_STOP, HIGH);
break;
}
}
// =====================================================
// DIRECTION DETECTION (from UWB) - with hysteresis
// =====================================================
DesiredDirection detectUserDirection() {
readUWB();
// If UWB not valid, maintain last known direction briefly
if (!uwbValid) {
return lastValidDirection;
}
// Apply hysteresis to prevent flickering at thresholds
DesiredDirection newDirection;
if (uwbAngle < -(TURN_ANGLE_THRESHOLD + HYSTERESIS_ANGLE)) {
newDirection = DIR_LEFT;
} else if (uwbAngle > (TURN_ANGLE_THRESHOLD + HYSTERESIS_ANGLE)) {
newDirection = DIR_RIGHT;
} else if (abs(uwbAngle) < (TURN_ANGLE_THRESHOLD - HYSTERESIS_ANGLE)) {
newDirection = DIR_FORWARD;
} else {
// In hysteresis band - keep previous direction
newDirection = lastValidDirection;
}
lastValidDirection = newDirection;
return newDirection;
}
// =====================================================
// SAFETY CHECK FUNCTIONS
// =====================================================
bool isLeftSideSafe() {
return sensorLeftValid && (distLeft >= SAFE_TURN_DISTANCE);
}
bool isRightSideSafe() {
return sensorRightValid && (distRight >= SAFE_TURN_DISTANCE);
}
bool isFrontClear() {
return sensorFrontValid && (distFront > FRONT_STOP_DISTANCE);
}
bool isRearDanger() {
// Something approaching from behind - might need to move forward
return sensorBackValid && (distBack < REAR_DANGER_DISTANCE);
}
bool isEmergencyStop() {
// Stop if: front obstacle too close OR sensor failure
if (!sensorFrontValid) return true; // Fail-safe: stop if sensor dead
if (distFront <= FRONT_STOP_DISTANCE) return true;
return false;
}
bool isBothSidesBlocked() {
return (distLeft < SAFE_TURN_DISTANCE && distRight < SAFE_TURN_DISTANCE);
}
bool isUserPresent() {
// Check if user is detected via UWB
return uwbValid && (uwbDistance < USER_LOST_DISTANCE);
}
// =====================================================
// STATE CHANGE HELPER (with debouncing)
// =====================================================
void changeState(TrolleyState newState) {
if (newState != currentState) {
// Debounce rapid state changes (except emergency)
if (newState != STATE_EMERGENCY_STOP &&
(millis() - lastStateChangeTime) < STATE_DEBOUNCE) {
return;
}
previousState = currentState;
currentState = newState;
lastStateChangeTime = millis();
}
}
// =====================================================
// SAFE TURNING DECISION LOGIC
// =====================================================
void handleTurningLogic() {
userDirection = detectUserDirection();
// Priority 0: Check if user is still present
if (!isUserPresent()) {
if (currentState != STATE_USER_LOST) {
userLostTime = millis();
changeState(STATE_USER_LOST);
}
return;
}
// Priority 1: Emergency collision prevention
if (isEmergencyStop()) {
changeState(STATE_EMERGENCY_STOP);
stopMotors();
return;
}
// Priority 1.5: Rear danger detection
if (isRearDanger() && isFrontClear()) {
changeState(STATE_REAR_DANGER);
return;
}
// Priority 2: Handle current turning states (don't interrupt mid-turn)
switch(currentState) {
case STATE_WAITING_TO_TURN_LEFT:
handleWaitingToTurnLeft();
return;
case STATE_WAITING_TO_TURN_RIGHT:
handleWaitingToTurnRight();
return;
case STATE_TURNING_LEFT:
handleActiveTurnLeft();
return;
case STATE_TURNING_RIGHT:
handleActiveTurnRight();
return;
case STATE_USER_LOST:
// User found again - resume following
changeState(STATE_FOLLOWING);
return;
case STATE_REAR_DANGER:
// If rear is clear now, go back to following
if (!isRearDanger()) {
changeState(STATE_FOLLOWING);
}
return;
default:
break;
}
// Priority 3: Check for new turn requests
if (userDirection == DIR_LEFT) {
if (isLeftSideSafe()) {
// Case 1: User turns left, left side clear → Immediate turn
changeState(STATE_TURNING_LEFT);
Serial.println("TURN_LEFT_SAFE - Executing turn");
} else {
// Case 2: User turns left, left side blocked → Wait
changeState(STATE_WAITING_TO_TURN_LEFT);
turnWaitStartTime = millis();
Serial.println("TURN_LEFT_BLOCKED - Waiting for clearance");
}
}
else if (userDirection == DIR_RIGHT) {
if (isRightSideSafe()) {
// Case 1: User turns right, right side clear → Immediate turn
changeState(STATE_TURNING_RIGHT);
Serial.println("TURN_RIGHT_SAFE - Executing turn");
} else {
// Case 3: User turns right, right side blocked → Wait
changeState(STATE_WAITING_TO_TURN_RIGHT);
turnWaitStartTime = millis();
Serial.println("TURN_RIGHT_BLOCKED - Waiting for clearance");
}
}
else {
// Forward following mode
changeState(STATE_FOLLOWING);
}
}
// Handle waiting to turn left (creep forward until clear)
void handleWaitingToTurnLeft() {
// Re-read UWB to check if user changed mind
readUWB();
// If user changed direction, abort waiting
if (uwbAngle > TURN_ANGLE_THRESHOLD) {
Serial.println("USER_CHANGED_DIRECTION - Aborting left wait");
changeState(STATE_FOLLOWING);
return;
}
// Check timeout
if (millis() - turnWaitStartTime > MAX_TURN_WAIT) {
Serial.println("TURN_WAIT_TIMEOUT - Resuming forward");
changeState(STATE_FOLLOWING);
return;
}
// Case 5: Obstacle in front while waiting → Stop
if (!isFrontClear()) {
stopMotors();
Serial.println("FRONT_BLOCKED_WHILE_WAITING - Stopped");
return;
}
// Check if left is now clear
if (isLeftSideSafe()) {
changeState(STATE_TURNING_LEFT);
Serial.println("LEFT_CLEARED - Initiating turn");
return;
}
// Continue creeping forward
creepForward();
Serial.println("CREEP_FORWARD - Waiting for left clearance");
}
// Handle waiting to turn right
void handleWaitingToTurnRight() {
// Re-read UWB to check if user changed mind
readUWB();
// If user changed direction, abort waiting
if (uwbAngle < -TURN_ANGLE_THRESHOLD) {
Serial.println("USER_CHANGED_DIRECTION - Aborting right wait");
changeState(STATE_FOLLOWING);
return;
}
if (millis() - turnWaitStartTime > MAX_TURN_WAIT) {
Serial.println("TURN_WAIT_TIMEOUT - Resuming forward");
changeState(STATE_FOLLOWING);
return;
}
if (!isFrontClear()) {
stopMotors();
Serial.println("FRONT_BLOCKED_WHILE_WAITING - Stopped");
return;
}
if (isRightSideSafe()) {
changeState(STATE_TURNING_RIGHT);
Serial.println("RIGHT_CLEARED - Initiating turn");
return;
}
creepForward();
Serial.println("CREEP_FORWARD - Waiting for right clearance");
}
// Active left turn with continuous safety monitoring
void handleActiveTurnLeft() {
// Re-read sensors for real-time safety
readUWB();
// Continuous safety check during turn
if (distLeft < SHELF_THRESHOLD || distFront < FRONT_STOP_DISTANCE) {
stopMotors();
changeState(STATE_EMERGENCY_STOP);
Serial.println("TURN_ABORTED - Obstacle detected");
return;
}
// Check if turn complete (user now in front) - with hysteresis
if (abs(uwbAngle) < (TURN_ANGLE_THRESHOLD / 2)) {
changeState(STATE_FOLLOWING);
Serial.println("TURN_COMPLETE - Resuming follow");
return;
}
turnLeft(SPEED_TURN);
}
// Active right turn with continuous safety monitoring
void handleActiveTurnRight() {
// Re-read sensors for real-time safety
readUWB();
if (distRight < SHELF_THRESHOLD || distFront < FRONT_STOP_DISTANCE) {
stopMotors();
changeState(STATE_EMERGENCY_STOP);
Serial.println("TURN_ABORTED - Obstacle detected");
return;
}
if (abs(uwbAngle) < (TURN_ANGLE_THRESHOLD / 2)) {
changeState(STATE_FOLLOWING);
Serial.println("TURN_COMPLETE - Resuming follow");
return;
}
turnRight(SPEED_TURN);
}
// =====================================================
// FOLLOWING MODE WITH SPEED CONTROL
// =====================================================
void handleFollowingMode() {
readToF();
// Use ToF for fine distance control when close
long effectiveDistance = (tofDistance > 0 && tofDistance < 100) ? tofDistance : distFront;
// FIXED LOGIC ORDER: Check closest distances first (most restrictive first)
// Priority 1: Too close to user - STOP
if (effectiveDistance <= USER_TOO_CLOSE && effectiveDistance > 0) {
stopMotors();
Serial.println("TOO_CLOSE - Maintaining distance");
return;
}
// Priority 2: Obstacle very close - STOP
if (effectiveDistance <= FRONT_STOP_DISTANCE) {
stopMotors();
Serial.println("STOP - Obstacle ahead");
return;
}
// Priority 3: Getting close - SLOW
if (effectiveDistance <= FRONT_SLOW_DISTANCE) {
moveForward(SPEED_SLOW);
Serial.println("FOLLOW_SLOW");
return;
}
// Priority 4: Medium distance - MEDIUM speed
if (effectiveDistance <= FRONT_MED_DISTANCE) {
moveForward(SPEED_MEDIUM);
Serial.println("FOLLOW_MEDIUM");
return;
}
// Priority 5: User far - FAST
if (effectiveDistance <= USER_TOO_FAR) {
moveForward(SPEED_FAST);
Serial.println("FOLLOW_FAST");
return;
}
// User too far - move at max speed to catch up
moveForward(SPEED_FAST);
Serial.println("CATCHING_UP - User far ahead");
}
// =====================================================
// MAIN STATE EXECUTION
// =====================================================
void executeState() {
switch(currentState) {
case STATE_FOLLOWING:
handleFollowingMode();
break;
case STATE_WAITING_TO_TURN_LEFT:
case STATE_WAITING_TO_TURN_RIGHT:
// Handled in turning logic - but ensure motors running
break;
case STATE_TURNING_LEFT:
case STATE_TURNING_RIGHT:
// Handled in turning logic
break;
case STATE_EMERGENCY_STOP:
stopMotors();
Serial.println("EMERGENCY_STOP");
// Recovery: check if safe to resume after brief pause
if (millis() - lastStateChangeTime > 500) { // 500ms pause
if (isFrontClear()) {
changeState(STATE_FOLLOWING);
Serial.println("EMERGENCY_CLEARED - Resuming");
}
}
break;
case STATE_BLOCKED:
stopMotors();
Serial.println("BLOCKED - Both sides obstructed");
if (!isBothSidesBlocked() && isFrontClear()) {
changeState(STATE_FOLLOWING);
}
break;
case STATE_USER_LOST:
// Gradually slow down then stop
if (millis() - userLostTime < 1000) {
moveForward(SPEED_SLOW); // Continue slowly for 1 sec
Serial.println("USER_LOST - Slowing down");
} else if (millis() - userLostTime < USER_LOST_TIMEOUT) {
moveForward(SPEED_CREEP); // Creep for another 2 sec
Serial.println("USER_LOST - Searching");
} else {
stopMotors();
Serial.println("USER_LOST - Stopped, waiting for user");
}
break;
case STATE_REAR_DANGER:
// Move forward slowly to escape rear danger
if (isFrontClear()) {
moveForward(SPEED_SLOW);
Serial.println("REAR_DANGER - Moving forward");
} else {
stopMotors();
Serial.println("REAR_DANGER - Front blocked, can't escape");
}
break;
case STATE_IDLE:
default:
stopMotors();
break;
}
}
// =====================================================
// DEBUG OUTPUT
// =====================================================
void printDebugInfo() {
static unsigned long lastPrintTime = 0;
// Only print every 500ms to avoid serial flooding
if (millis() - lastPrintTime < 500) return;
lastPrintTime = millis();
Serial.print("F:"); Serial.print(distFront);
Serial.print(sensorFrontValid ? "" : "!"); // ! = sensor error
Serial.print(" L:"); Serial.print(distLeft);
Serial.print(sensorLeftValid ? "" : "!");
Serial.print(" R:"); Serial.print(distRight);
Serial.print(sensorRightValid ? "" : "!");
Serial.print(" B:"); Serial.print(distBack);
Serial.print(sensorBackValid ? "" : "!");
Serial.print(" | UWB:"); Serial.print(uwbAngle);
Serial.print("°"); Serial.print(uwbValid ? "" : "(N/A)");
Serial.print(" | State:");
switch(currentState) {
case STATE_IDLE: Serial.println("IDLE"); break;
case STATE_FOLLOWING: Serial.println("FOLLOWING"); break;
case STATE_WAITING_TO_TURN_LEFT: Serial.println("WAIT_LEFT"); break;
case STATE_WAITING_TO_TURN_RIGHT: Serial.println("WAIT_RIGHT"); break;
case STATE_TURNING_LEFT: Serial.println("TURN_LEFT"); break;
case STATE_TURNING_RIGHT: Serial.println("TURN_RIGHT"); break;
case STATE_EMERGENCY_STOP: Serial.println("E_STOP"); break;
case STATE_BLOCKED: Serial.println("BLOCKED"); break;
case STATE_USER_LOST: Serial.println("USER_LOST"); break;
case STATE_REAR_DANGER: Serial.println("REAR_DANGER"); break;
default: Serial.println("UNKNOWN"); break;
}
}
// =====================================================
// SETUP
// =====================================================
void setup() {
Serial.begin(115200);
delay(100);
Serial.println();
Serial.println("╔═══════════════════════════════════════════════╗");
Serial.println("║ SMART SHOPPING TROLLEY - Initializing... ║");
Serial.println("╚═══════════════════════════════════════════════╝");
// Ultrasonic sensors
pinMode(TF, OUTPUT); pinMode(EF, INPUT);
pinMode(TL, OUTPUT); pinMode(EL, INPUT);
pinMode(TR, OUTPUT); pinMode(ER, INPUT); // ER = GPIO 36 (input only)
pinMode(TB, OUTPUT); pinMode(EB, INPUT); // EB = GPIO 39 (input only)
Serial.println("[OK] Ultrasonic sensors configured");
// LED indicators
pinMode(LED_FAST, OUTPUT);
pinMode(LED_MED, OUTPUT);
pinMode(LED_SLOW, OUTPUT);
pinMode(LED_STOP, OUTPUT);
// Test LEDs
digitalWrite(LED_FAST, HIGH); delay(100);
digitalWrite(LED_MED, HIGH); delay(100);
digitalWrite(LED_SLOW, HIGH); delay(100);
digitalWrite(LED_STOP, HIGH); delay(100);
digitalWrite(LED_FAST, LOW);
digitalWrite(LED_MED, LOW);
digitalWrite(LED_SLOW, LOW);
digitalWrite(LED_STOP, LOW);
Serial.println("[OK] LED indicators configured");
// Motors
setupMotors();
Serial.println("[OK] Motor driver configured");
// Initialize I2C for UWB (if using I2C mode)
// Wire.begin(UWB_SDA, UWB_SCL);
// initUWB();
Serial.println("[--] UWB module - using simulation mode");
Serial.println(" Send: L45 (left 45°), R30 (right 30°), F (forward), X (lost)");
// Initialize UART for ToF (if using GY-53)
// Serial2.begin(9600, SERIAL_8N1, TOF_RX, TOF_TX);
// initToF();
Serial.println("[--] ToF sensor - using ultrasonic fallback");
// Initial state
currentState = STATE_IDLE;
uwbValid = true; // Assume valid for simulation
// Startup sensor check
Serial.println();
Serial.println("Running sensor self-test...");
readAllSensors();
Serial.print(" Front: "); Serial.print(distFront); Serial.println(sensorFrontValid ? " cm [OK]" : " cm [FAIL]");
Serial.print(" Left: "); Serial.print(distLeft); Serial.println(sensorLeftValid ? " cm [OK]" : " cm [FAIL]");
Serial.print(" Right: "); Serial.print(distRight); Serial.println(sensorRightValid ? " cm [OK]" : " cm [FAIL]");
Serial.print(" Back: "); Serial.print(distBack); Serial.println(sensorBackValid ? " cm [OK]" : " cm [FAIL]");
Serial.println();
Serial.println("╔═══════════════════════════════════════════════╗");
Serial.println("║ System Ready - Starting in 2 seconds... ║");
Serial.println("╚═══════════════════════════════════════════════╝");
digitalWrite(LED_STOP, HIGH);
delay(2000);
digitalWrite(LED_STOP, LOW);
changeState(STATE_FOLLOWING);
Serial.println("Mode: FOLLOWING");
}
// =====================================================
// MAIN LOOP
// =====================================================
void loop() {
// Step 1: Read all sensors
readAllSensors();
// Step 2: Process turning decision logic
handleTurningLogic();
// Step 3: Execute current state
executeState();
// Step 4: Update LED indicators
updateLEDs(currentState);
// Step 5: Debug output (rate-limited internally)
printDebugInfo();
delay(50); // 20Hz control loop (faster for better response)
}