#define ENA 5

#define ENB 6
#define in1 7
#define in2 8
#define in3 9
#define in4 10
int cambientrai;
int cambientruoc;
int cambienphai;
int hientai=0, quakhu=0 ;
int TT;
float XungENB=0,XungENA=0;
void doccambien()
{
cambientrai=analogRead(A0);
cambientruoc=analogRead(A1);
cambienphai=analogRead(A2);
Serial.print(" Trai "); //70
Serial.print(cambientrai);
Serial.print(" Truoc "); //50
Serial.print(cambientruoc);
Serial.print(" Phai "); //40
Serial.println(cambienphai);
}
//============Thiết kế cho xe quẹo trái==========
void QueoTrai() // cho bám phải
{ nghichtrai();
nghichphai();
analogWrite(ENA,150);//110 // giá trị đo thực tế
 analogWrite(ENB,150);
delay(10);
while (cambientruoc<560)
{
thuanphai();
nghichtrai();
analogWrite(ENA,100);//110 // giá trị đo thực tế
analogWrite(ENB,100);
doccambien();
}
}
void QueoPhai() // cho bám trái
{
nghichtrai();
nghichphai();
analogWrite(ENA,150);//110 // giá trị đo thực tế
analogWrite(ENB,150);
delay(10);
while (cambientruoc<560)
{
thuantrai();
nghichphai();
analogWrite(ENA,100);//110 // giá trị đo thực tế
analogWrite(ENB,100);
doccambien();
}
}

void nghichphai()
{
digitalWrite(in1,LOW);
digitalWrite(in2,HIGH);
}
void thuanphai()
{
digitalWrite(in2,LOW);
digitalWrite(in1,HIGH);
}
void nghichtrai()
{
digitalWrite(in3,LOW);
digitalWrite(in4,HIGH);
}
void thuantrai()
{
digitalWrite(in4,LOW);
digitalWrite(in3,HIGH);
}
void BamPhai() // bám phải trái cộng, phải trừ
{ float Kp = 8.5 , Kd = 2.2, Ki = 0.05; // Kp = 9.5, Kd = 1.2, Ki = 0;
float P, I, D;
static int lasterror;
int out;
float SamplingTime = 0.01;
int error = 750 - cambienphai;
thuantrai(); thuanphai();
P = error * Kp;
I += Ki * error * SamplingTime;
 D = (Kd * (error - lasterror )) / SamplingTime;
out = P + I + D;
lasterror = error;
XungENB = 60 - out; //XungENB = 50 - out;
XungENA = 60 + out; //XungENA = 50 + out;
if (XungENB > 255) XungENB = 255;
else if (XungENB < 0) XungENB = 0;
if (XungENA > 255) XungENA = 255;
else if (XungENA < 0) XungENA = 0;
analogWrite(ENB, XungENB );//100
analogWrite(ENA, XungENA);//100
}
void BamTrai() // bám trái là trái trừ, phải cộng (XungENB,XungENA)
{
float Kp = 16 , Kd =1.1 , Ki = 0.1;//float Kp = 15 , Kd =0.75 , Ki = 0;
float P, I, D;
static int lasterror;
int out;
float SamplingTime = 0.01;
int error = 680 - cambientrai;
thuantrai(); thuanphai();
P = error * Kp;
I += Ki * error * SamplingTime;
D = (Kd * (error - lasterror )) / SamplingTime;
out = P + I + D;
lasterror = error;
XungENA = 60 - out; // XungENA = 50 - out;
XungENB = 60 + out; // XungENB = 50 + out;
if (XungENB > 255) XungENB = 255;
 else if (XungENB < 0) XungENB = 0;

if (XungENA > 255) XungENA = 255;

else if (XungENA < 0) XungENA = 0;

analogWrite(ENB, XungENB); //100

analogWrite(ENA, XungENA); //100
}
void setup()
{
Serial.begin(9600);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(in1,OUTPUT);
pinMode(in2,OUTPUT);
pinMode(in3,OUTPUT);
pinMode(in4,OUTPUT);
}
void loop()
{
hientai=millis();
if(hientai-quakhu>=5000)
{ TT=1; quakhu=hientai; }
else
TT=0;
doccambien();
if(TT==0)
{
BamPhai();
 if(cambientruoc<560)
{
QueoTrai();
}
}
if(TT==1)
{
BamTrai();
if(cambientruoc<560)
{
QueoPhai();
}
}
}

Code 2: