#include<Servo.

h>
Servo sv;
int in1=5;
int in2=4;
int in3=6;
int in4=7;
int trig = 12;
int echo = 13;
unsigned long t;
int dis, distrai,disphai;
int i;
void tien(int a)
{
digitalWrite(in1,a);
analogWrite(in2,LOW);
//motor A
digitalWrite(in3,a);
analogWrite(in4,LOW);
delay(500);
dung();
}
void dung()
{
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,LOW);
}
void lui(int b)
{
digitalWrite(in4,HIGH);
analogWrite(in3,b);
//motor A
digitalWrite(in2,HIGH);
analogWrite(in1,b);
delay(500);
dung();
}
void setup()
{
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4,OUTPUT);
pinMode(trig,OUTPUT);
pinMode(echo,INPUT);
sv.attach(9);
sv.write(90);
delay(500);

void loop()
{
dis = 0;
khoangcach();
if (dis>20 || dis == 0)
{
tien(50);
 }
else
{
dung();
khoangcachtrai();
khoangcachphai();
if(distrai<20&&disphai<20)
{
dung();
delay(50);
lui(50);
}
else{
if(disphai>=distrai)
{
dung();
delay(500);
quayphai();
delay(300);
tien(50);
}
if(distrai>disphai)
{
dung();
delay(500);
quaytrai();
delay(300);
tien(50);
}
}
}
}
void khoangcachtrai()
{
sv.write(0);
delay(333);
distrai=dis;
}
void khoangcachphai()
{
sv.write(180);
delay(333);
disphai=dis;
}
void quayphai()
{
digitalWrite(in1,HIGH);
digitalWrite(in2,LOW);
digitalWrite(in3,LOW);
digitalWrite(in4,LOW);
delay(500);
dung();
}
void quaytrai()
{
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
digitalWrite(in1,LOW);
digitalWrite(in2,LOW);
 delay(500);
dung();

void khoangcach()
{
digitalWrite(trig,LOW);
delayMicroseconds(2);
digitalWrite(trig,HIGH);
delayMicroseconds(5);
digitalWrite(trig,LOW);
t = pulseIn(echo,HIGH);
dis = int (t/2/29.141);
}