Obstacle Avoiding Car

/*
 * Obstacle Avoiding Car — Arduino
 *
 * Components: Arduino Uno, HC-SR04, L298N, SG90 Servo, 2x DC Motors
 *
 * How it works:
 * 1. Car drives forward
 * 2. Ultrasonic sensor measures distance ahead
 * 3. If obstacle detected (< 25cm):
 *    a. Stop
 *    b. Servo scans left and right
 *    c. Compare distances
 *    d. Turn toward the clearer side
 * 4. Repeat
 *
 * Watni Digital STEM Lab
 * https://www.watnidigital.com/education/stem-lab
 */

#include <Servo.h>

// === PIN DEFINITIONS ===
// Ultrasonic Sensor
#define TRIG_PIN 9
#define ECHO_PIN 10

// Motor Driver L298N
#define IN1 2   // Left motor
#define IN2 3
#define IN3 4   // Right motor
#define IN4 5
#define ENA 11  // Left motor speed (PWM)
#define ENB 12  // Right motor speed

// Servo
#define SERVO_PIN 6

// === SETTINGS ===
#define OBSTACLE_DISTANCE 25  // cm — stop if obstacle closer than this
#define MOTOR_SPEED 180       // 0-255 — adjust for your motors
#define TURN_DURATION 400     // ms — how long to turn

Servo scanServo;

void setup() {
  Serial.begin(9600);

  // Motor pins
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  // Ultrasonic pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Servo
  scanServo.attach(SERVO_PIN);
  scanServo.write(90); // Face forward
  delay(1000);

  Serial.println("Obstacle Avoiding Car Ready!");
}

void loop() {
  int distance = measureDistance();
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance > OBSTACLE_DISTANCE) {
    // Path is clear — drive forward
    moveForward();
  } else {
    // Obstacle detected!
    stopMotors();
    delay(300);

    // Scan right
    scanServo.write(30);
    delay(500);
    int rightDistance = measureDistance();
    Serial.print("Right: ");
    Serial.println(rightDistance);

    // Scan left
    scanServo.write(150);
    delay(500);
    int leftDistance = measureDistance();
    Serial.print("Left: ");
    Serial.println(leftDistance);

    // Return to center
    scanServo.write(90);
    delay(300);

    // Decide which way to turn
    if (rightDistance > leftDistance && rightDistance > OBSTACLE_DISTANCE) {
      // Turn right
      turnRight();
      delay(TURN_DURATION);
    } else if (leftDistance > OBSTACLE_DISTANCE) {
      // Turn left
      turnLeft();
      delay(TURN_DURATION);
    } else {
      // Both sides blocked — reverse and turn
      moveBackward();
      delay(500);
      turnRight();
      delay(TURN_DURATION * 2);
    }

    stopMotors();
    delay(200);
  }

  delay(50); // Small delay between readings
}

// === MOTOR FUNCTIONS ===

void moveForward() {
  analogWrite(ENA, MOTOR_SPEED);
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void moveBackward() {
  analogWrite(ENA, MOTOR_SPEED);
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void turnLeft() {
  analogWrite(ENA, MOTOR_SPEED);
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void turnRight() {
  analogWrite(ENA, MOTOR_SPEED);
  analogWrite(ENB, MOTOR_SPEED);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void stopMotors() {
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// === SENSOR FUNCTION ===

int measureDistance() {
  // Send ultrasonic pulse
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read echo time
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // 30ms timeout

  // Calculate distance in cm
  int distance = duration * 0.034 / 2;

  // Return max 200 if no echo (no obstacle)
  if (distance == 0 || distance > 200) {
    return 200;
  }

  return distance;
}