Build an Obstacle Avoiding Car with Arduino + L298N + HC-SR04 + SG90 Servo (Full Guide)

// Simple Obstacle Avoidance Car - Backward First, Then Scan
// Motors run at full speed only
// Complete Working Code - Do Not Modify

#include <Servo.h>

// ========== MOTOR PINS ==========
const int IN1 = 9;
const int IN2 = 8;
const int IN3 = 7;
const int IN4 = 6;

// ========== SERVO & ULTRASONIC ==========
const int SERVO_PIN = 3;
const int TRIG_PIN = 12;
const int ECHO_PIN = 13;

// ========== SETTINGS ==========
const int SAFE_DISTANCE = 30; // cm - detect obstacle
const int BACKWARD_DURATION = 150; // ms - how long to go backward
const int TURN_DURATION = 300; // ms - how long to turn
const int SCAN_DELAY = 150; // ms - delay for servo movement

// Scanning angles
const int ANGLE_LEFT = 180; // Full left
const int ANGLE_CENTER = 90; // Center
const int ANGLE_RIGHT = 0; // Full right

Servo myServo;
bool servoAtCenter = true;

void setup() {
// Motor pins
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);

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

// Servo
myServo.attach(SERVO_PIN);
myServo.write(ANGLE_CENTER);
delay(500);

Serial.begin(9600);

// Start moving forward
moveForward();
}

void loop() {
int distance = getDistance();

if (distance < SAFE_DISTANCE) {
// Obstacle detected - avoid it
avoidObstacle();
} else {
// Make sure servo stays at center during normal driving
if (!servoAtCenter) {
myServo.write(ANGLE_CENTER);
servoAtCenter = true;
delay(50);
}
moveForward();
}
}

// ========== MOTOR FUNCTIONS ==========

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

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

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

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

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

// ========== ULTRASONIC FUNCTIONS ==========

int getDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

long duration = pulseIn(ECHO_PIN, HIGH, 30000);

if (duration == 0) return 999;
return duration * 0.034 / 2;
}

int scanAngle(int angle) {
myServo.write(angle);
delay(SCAN_DELAY);
return getDistance();
}

// ========== OBSTACLE AVOIDANCE LOGIC ==========

void avoidObstacle() {
// Step 1: Stop the car
stopCar();
delay(50);

// Step 2: Go BACKWARD a few steps
Serial.println("Obstacle! Moving backward...");
moveBackward();
delay(BACKWARD_DURATION);

// Step 3: Stop
stopCar();
delay(50);

// Step 4: Scan LEFT and RIGHT
Serial.println("Scanning left and right...");

int leftDist = scanAngle(ANGLE_LEFT); // Scan left first
delay(500); // Wait for servo to complete rotation

int rightDist = scanAngle(ANGLE_RIGHT); // Scan right
delay(500); // Wait for servo to complete rotation

// Step 5: Return servo to CENTER
myServo.write(ANGLE_CENTER);
servoAtCenter = true;
delay(300); // Wait for servo to return to center

// Step 6: Turn toward clearer path
Serial.print("Left: ");
Serial.print(leftDist);
Serial.print("cm | Right: ");
Serial.print(rightDist);
Serial.println("cm");

if (leftDist > rightDist) {
Serial.println("Turning LEFT");
turnLeft();
} else {
Serial.println("Turning RIGHT");
turnRight();
}

delay(TURN_DURATION);

// Step 7: Stop turning
stopCar();
delay(50);

// Step 8: Resume forward
Serial.println("Moving forward again...");
moveForward();
}