#include <Servo.h>
const int pingPin= 7;
const int danger= 10;
int motorpin1= 3;
int motorpin2= 4;
int enablepin= 9;
int motorpin3= 5;
int motorpin4= 6;
int enablepin2= 10;
Servo myservo;
int leftdist, rightdist;
long duration;
void setup()
{
pinMode(motorpin1, OUTPUT);
pinMode(motorpin2, OUTPUT);
pinMode(enablepin, OUTPUT);
pinMode(motorpin3, OUTPUT);
pinMode(motorpin4, OUTPUT);
pinMode(enablepin2, OUTPUT);
digitalWrite(enablepin, HIGH);
digitalWrite(enablepin2, HIGH);
myservo.attach(8);
myservo.attach(9);
}
void loop()
{
int distanceFwd= ping();
if (distanceFwd>danger)
{
digitalWrite(motorpin1, LOW);
digitalWrite(motorpin2, HIGH);
digitalWrite(motorpin3, LOW);
digitalWrite(motorpin4, HIGH);
}
else
{
digitalWrite(motorpin1, LOW);
digitalWrite(motorpin2, LOW);
digitalWrite(motorpin3, LOW);
digitalWrite(motorpin4, LOW);
myservo.write(0);
delay(500);
rightdist= ping();
delay(500);
myservo.write(180);
delay(700);
leftdist= ping();
delay(500);
myservo.write(90);
delay(100);
compareDistance();
}
}
void compareDistance()
{
if (leftdist>rightdist)
{
digitalWrite(motorpin1, LOW);
digitalWrite(motorpin2, HIGH);
digitalWrite(motorpin3, HIGH);
digitalWrite(motorpin4, LOW);
delay(500);
}
else if (rightdist>leftdist)
{
digitalWrite(motorpin1, HIGH);
digitalWrite(motorpin2, LOW);
digitalWrite(motorpin3, LOW);
digitalWrite(motorpin4, HIGH);
delay(500);
}
else
{
digitalWrite(motorpin1, HIGH);
digitalWrite(motorpin2, LOW);
digitalWrite(motorpin3, LOW);
digitalWrite(motorpin4, HIGH);
delay(1000);
}
}
long ping()
{
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration= pulseIn(pingPin, HIGH);
return duration / 29 / 2;
}