// Pin Definitions
const int trigPin = 9;
const int echoPin = 10;
const int motor1Pin1 = 5;
const int motor1Pin2 = 6;
const int motor2Pin1 = 7;
const int motor2Pin2 = 8;
// Define variables
long duration;
int distance;
void setup() {
// Initialize motor pins
pinMode(motor1Pin1, OUTPUT);
pinMode(motor1Pin2, OUTPUT);
pinMode(motor2Pin1, OUTPUT);
pinMode(motor2Pin2, OUTPUT);
// Initialize ultrasonic sensor pins
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Start serial communication
Serial.begin(9600);
}
void loop() {
// Send pulse to the ultrasonic sensor
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the pulse from the echo pin
duration = pulseIn(echoPin, HIGH);
// Calculate distance (in cm)
distance = duration * 0.0344 / 2;
// Print distance to serial monitor for debugging
Serial.print("Distance: ");
Serial.println(distance);
// If obstacle is detected within 20 cm
if (distance < 20) {
// Stop motors
stopMotors();
// Backup for 1 second
moveBackward();
delay(1000);
// Turn for 1 second
turnRight();
delay(1000);
// Move forward after turning
moveForward();
} else {
// If no obstacle, move forward
moveForward();
}
// Small delay before next loop
delay(100);
}
void moveForward() {
digitalWrite(motor1Pin1, HIGH);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, HIGH);
digitalWrite(motor2Pin2, LOW);
}
void moveBackward() {
digitalWrite(motor1Pin1, LOW);
digitalWrite(motor1Pin2, HIGH);
digitalWrite(motor2Pin1, LOW);
digitalWrite(motor2Pin2, HIGH);
}
void stopMotors() {
digitalWrite(motor1Pin1, LOW);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, LOW);
digitalWrite(motor2Pin2, LOW);
}
void turnRight() {
digitalWrite(motor1Pin1, HIGH);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, LOW);
digitalWrite(motor2Pin2, HIGH);
}
