#include <Servo.h>
 
int triger=12;
int echo=13;

long duration;
int distance;

 Servo myservo;
int pos=90;
int a,b;

void ultrasonic();

void forward()
{
  digitalWrite(4,HIGH); // Turn forward
digitalWrite(5,LOW);
digitalWrite(9,HIGH);
digitalWrite(10,LOW);
}


void setup() 
{
myservo.attach(3);
pinMode(triger,OUTPUT);
pinMode(echo, INPUT);
pinMode(6,OUTPUT);//enable
pinMode(4,OUTPUT);//A1
pinMode(5,OUTPUT);//A2
pinMode(9,OUTPUT);//B2
pinMode(10,OUTPUT);//B1
pinMode(11,OUTPUT);//enable
Serial.begin(9600);
analogWrite(6,200);// enable 
analogWrite(11, 200);//enable
}
void loop() {
  // put your main code here, to run repeatedly:
   ultrasonic();


if(distance<10)
{
 digitalWrite(4,LOW);
 digitalWrite(5,LOW);
 digitalWrite(9,LOW);
 digitalWrite(10,LOW);
 delay(500);
 myservo.write(180); 
 a=distance;
 delay(15);
 myservo.write(0);
 b=distance;
 delay(15);
myservo.write(90);
 delay(15);

if(a<b)
{

digitalWrite(4,HIGH);// Turn RIGHT
digitalWrite(5,LOW);
digitalWrite(9,LOW);
digitalWrite(10,LOW);

delay(1000);
forward();
}
else if(a>b)
{
digitalWrite(4,LOW);// Turn left
digitalWrite(5,LOW);
digitalWrite(9,HIGH);
digitalWrite(10,LOW);
delay(1000);
forward();
}
}
else if(distance>10)
{
digitalWrite(4,HIGH);// forward
digitalWrite(5,LOW);
digitalWrite(9,HIGH);
digitalWrite(10,LOW);
}
}

void ultrasonic()
{
  digitalWrite(triger, LOW);
delayMicroseconds(2);

digitalWrite(triger, HIGH);
delayMicroseconds(10);
digitalWrite(triger, LOW);
duration= pulseIn(echo,HIGH);
distance= duration * 0.034/2;
Serial.print("Distance=");
Serial.println(distance);
}