#include <Servo.h>
#define SPEED_OF_SOUND 0.034
const int trigPin = 4;
const int echoPin = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
Servo servo;

void setup() {
  servo.attach(3);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
  Serial.begin(9600);
}
void loop() {
servo.write(90);

      long duration;
        int distance;
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);

        duration = pulseIn(echoPin, HIGH);
        distance= duration*(SPEED_OF_SOUND/2);
        Serial.print("Distance: ");
        Serial.println(distance);
        delay(100);

digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);

 if (distance < 25) {
 Serial.println("back up");
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
delay(250);
Serial.println("stop");
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
  servo.write(180);
      int distancel;
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);
        duration = pulseIn(echoPin, HIGH);
        distancel = duration*(SPEED_OF_SOUND/2);
        Serial.print("Distancel: ");
        Serial.println(distancel);
     delay(500);
     servo.write(0);
          int distancer;
        digitalWrite(trigPin, LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin, LOW);

        duration = pulseIn(echoPin, HIGH);
        distancer = duration*(SPEED_OF_SOUND/2);
       Serial.print("Distancer: ");
        Serial.println(distancer);
        delay(500);

          if(distancer > distancel) {
          Serial.println("right ");
          digitalWrite(IN1, LOW);
          digitalWrite(IN2, HIGH);
          digitalWrite(IN3, LOW);
          digitalWrite(IN4, HIGH);
          delay(1500); }
            else{
               Serial.println("left ");
            digitalWrite(IN1, HIGH);
            digitalWrite(IN2, LOW);
            digitalWrite(IN3, HIGH);
            digitalWrite(IN4, LOW);
            delay(1500); }

        }
        }