#include <Servo.h>
#include <NewPing.h>
Servo servo;
NewPing sonar(A5, A4); // Trigger pin, Echo pin
const int servoPin = 9; // Servo motor control pin
const int safeDistance = 10; // Safe distance in centimeters
const int leftMotor1 = 6; // Change to your desired pin for left motor control pin 1
const int leftMotor2 = 7; // Change to your desired pin for left motor control pin 2
const int rightMotor1 = 8; // Change to your desired pin for right motor control pin 1
const int rightMotor2 = 5; // Change to your desired pin for right motor control pin 2
int initialAngle = 180; // Initial servo angle
void setup() {
servo.attach(servoPin);
servo.write(initialAngle); // Start with the servo pointing straight ahead
// Set motor pins as outputs
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
}
void loop() {
int distance = sonar.ping_cm();
if (distance <= safeDistance) {
// If an object is detected within the safe distance, rotate the servo to find a clear path
int clearPathAngle = findClearPath();
// Rotate back to the initial position
servo.write(initialAngle);
// Implement motor control based on the clear path angle
if (clearPathAngle < 90) {
// Turn right
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, LOW);
} else if (clearPathAngle > 90) {
// Turn left
digitalWrite(leftMotor1, LOW);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
} else {
// Move forward
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
}
} else {
// If there is no object within the safe distance, move forward
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, HIGH);
digitalWrite(rightMotor2, LOW);
}
}
int findClearPath() {
int clearPathAngle = -1; // Initialize to an invalid value
int minDistance = 9999; // Initialize to a high value
for (int angle = 0; angle <= 180; angle += 90) {
servo.write(angle); // Rotate the servo in 90-degree intervals
delay(1000); // Delay for a moment to allow the servo to settle
int distance = sonar.ping_cm();
if (distance > safeDistance) {
// If a clear path is found, record the clear path angle
clearPathAngle = angle;
break; // Exit the loop when a clear path is found
} else if (distance < minDistance) {
// If no clear path is found, record the minimum distance
minDistance = distance;
}
}
if (clearPathAngle == -1) {
// If no clear path is found, return the servo to its initial position
clearPathAngle = initialAngle;
}
return clearPathAngle;
}