Arduino 2WD Obstacle Avoiding Vehicle

 /* Development of an Autonomous Vehicle Using Arduino for Obstacle Avoidance
 */

#define TRIG_PIN 13 // Ultrasonic Sensor Trigger Pin Definition
#define ECHO_PIN 12 // Ultrasonic Sensor Echo Pin Definition

// Motor Control Pins Definition (L293D to Arduino)
// LEFT MOTOR
#define ENABLE_LEFT  11   // Pin 1 on L293D (Speed/On-Off)
#define MOTOR_LEFT_1 10   // Pin 2 on L293D
#define MOTOR_LEFT_2 9    // Pin 7 on L293D

// RIGHT MOTOR
#define ENABLE_RIGHT 6    // Pin 9 on L293D (Speed/On-Off)
#define MOTOR_RIGHT_1 5   // Pin 10 on L293D
#define MOTOR_RIGHT_2 4   // Pin 15 on L293D

// GO - NO GO INDICATORS
#define LED_GREEN 3       // Safe to move forward
#define LED_RED   2       // Obstacle Detected below the safe distance
#define BUZZER    8       // Warning Alarm

// CONSTANTS and VARIABLES
#define SAFE_DISTANCE 20 // Safe limit distance in centimeters (cm)
#define TURN_DELAY 800 // Delay to allow the car to turn right

long duration; // Time of flight of the sound wave (ms)
int distance;  // Calculated distance in cm

void setup() {
  
  Serial.begin(9600); // Initialize Serial Monitor at 9600 bit rate

  pinMode(ENABLE_LEFT, OUTPUT); // Configure Left Motor Pins - Power On/Off
  pinMode(MOTOR_LEFT_1, OUTPUT); // Forward Spin
  pinMode(MOTOR_LEFT_2, OUTPUT); // Backward Spin
  
  pinMode(ENABLE_RIGHT, OUTPUT); // Configure Right Motor Pins - Power On/Off
  pinMode(MOTOR_RIGHT_1, OUTPUT); // Forward Spin
  pinMode(MOTOR_RIGHT_2, OUTPUT); // Backward Spin

  pinMode(TRIG_PIN, OUTPUT); // Configure Ultra Sonic Sensor Pin TRIGGER
  pinMode(ECHO_PIN, INPUT); // Configure Ultra Sonic Sensor Pin ECHO

  pinMode(LED_GREEN, OUTPUT); // Configure GREEN LED - GO Indicator
  pinMode(LED_RED, OUTPUT); // Configure RED LED - NO GO Indicator
  pinMode(BUZZER, OUTPUT); // Configure PIEZO buzzer for Obstacle Audio Warning 

  stopMotors(); // Ensure motors are stopped at startup for safety
}

void loop() {
  
  distance = getDistance(); // Measure distance in front of vehicle
  //Debugging
  Serial.print("Distance: "); // Print obstacle distance to Serial Monitor
  Serial.print(distance);
  Serial.println(" cm");
  delay(100); // Short delay for monitor readability
 
  if (distance < SAFE_DISTANCE) {  // Decision Making according to obstacle detection
  
    // Obstacle Detection (closer than 20cm) Indicators: Red ON, Green OFF, Buzzer ON
    digitalWrite(LED_RED, HIGH);
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(BUZZER, LOW); // Buzzer Module Low Level Trigger (LOW=ON)
    
    // Execute Avoidance Maneuvers
    stopMotors(); // 1. Both motors stop - Vehicle stop
    delay(1000); 
    
    moveBackward(); // 2. Both motors move the vehicle backwards
    delay(1000); 
    
    turnRight();  // 3. Turn right to avoid obstacle (left motor spin forward - right motor spin backwards)
    delay(TURN_DELAY); 
    
    stopMotors(); // Both motors stop - Vehicle stop before checking again
  
  } else {
        
    // Obstacle NOT Detected (Route Clear) Indicators: Green ON, Red OFF, Buzzer OFF
    digitalWrite(LED_GREEN, HIGH);
    digitalWrite(LED_RED, LOW);
    digitalWrite(BUZZER, HIGH); //Buzzer Module Low Level Trigger (HIGH=OFF)
    
    moveForward(); // Move Forward
  }
  
}

// MANEUVERS CONTROL FUNCTIONS

void moveForward() {
  //Both Motors Forward - Vehicle Moves FORWARD
  digitalWrite(ENABLE_LEFT, HIGH); // Enable Left Motor (Power ON)
  digitalWrite(ENABLE_RIGHT, HIGH); // Enable Right Motor (Power ON)
  
  // Left Motor Forward
  digitalWrite(MOTOR_LEFT_1, HIGH); // Left Motor Forward Spin ON
  digitalWrite(MOTOR_LEFT_2, LOW); // Left Motor Backward  Spin OFF
  
  // Right Motor Forward
  digitalWrite(MOTOR_RIGHT_1, HIGH); // Right Motor Forward Spin ON
  digitalWrite(MOTOR_RIGHT_2, LOW); // Right Motor Backward Spin OFF
}

void moveBackward() {
  // Both Motors Backward - Vehicle Moves BACKWARD
  digitalWrite(ENABLE_LEFT, HIGH); // Enable Left Motor (Power ON)
  digitalWrite(ENABLE_RIGHT, HIGH); // Enable Right Motor (Power ON)
  
  // Left Motor Backward
  digitalWrite(MOTOR_LEFT_1, LOW); //Left Motor Forward  Spin OFF
  digitalWrite(MOTOR_LEFT_2, HIGH); //Left Motor Backward  Spin ON
  
  // Right Motor Backward
  digitalWrite(MOTOR_RIGHT_1, LOW); //Right Motor Forward  Spin OFF
  digitalWrite(MOTOR_RIGHT_2, HIGH); //Right Motor Backward  Spin ON
}

void turnRight() {
  // Left Motor Forward & Right Motor Backward -- Vehicle Turns Right
  digitalWrite(ENABLE_LEFT, HIGH); // Enable Left Motor (Power ON)
  digitalWrite(ENABLE_RIGHT, HIGH); // Enable Right Motor (Power ON)
  
  // Left Motor Forward
  digitalWrite(MOTOR_LEFT_1, HIGH); // Left Motor Forward Spin ON
  digitalWrite(MOTOR_LEFT_2, LOW); // Left Motor Backward Spin OFF
  
  // Right Motor Backward
  digitalWrite(MOTOR_RIGHT_1, LOW); //Right Motor Forward  Spin OFF
  digitalWrite(MOTOR_RIGHT_2, HIGH); //Right Motor Backward  Spin ON
}

void stopMotors() {
  
  digitalWrite(ENABLE_LEFT, LOW); // Disable Left Motor (Power OFF)
  digitalWrite(ENABLE_RIGHT, LOW); // Disable Right Motor (Power OFF)
    
  digitalWrite(MOTOR_LEFT_1, LOW); // Set LEFT Forward spin Off for safety
  digitalWrite(MOTOR_LEFT_2, LOW); // Set LEFT Backward spin Off for safety
  digitalWrite(MOTOR_RIGHT_1, LOW); // Set RIGHT Forward spin Off for safety
  digitalWrite(MOTOR_RIGHT_2, LOW); // Set RIGHT Backward spin Off safety
}

// Ultra Sonic Sensor HC-SR04 FUNCTION

int getDistance() { // Obstacle Distance Calculation
  
  digitalWrite(TRIG_PIN, LOW); // 1. Clear Trigger (Off)
  delayMicroseconds(2);
  
  digitalWrite(TRIG_PIN, HIGH); // 2. Send Pulse (8 pulses- 40KHz)
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW); // Trigger Pulse Off
    
  duration = pulseIn(ECHO_PIN, HIGH); // 3. Read Echo and save value in duration variable
  
  return duration * 0.034 / 2;// Distance Calculation in cm (Time x Speed of Sound / 2)
}