//CHUONG TRINH DIEU KHIEN XE (iCAR CONTEST PROJECT )
//----------------------------------------------------------------------------------------------#include
#define FORWARD 1
// Lua chon cho xce ten
#define BACKWARD 2
// Lua chon cho xce lui
#define LEFT 3
// Lua chon cho xce re tra i
#define RIGHT 4
// Lua chon cho xce re pha i
#define lihhea d A1
// Den truoc
#define lihrea r A2
// Den sa u
#define lihL A0
// Xinha n Le羐
#define lihR A3
// Xinha n Right
Servo myservo1;
// Ta o doi tuong dieu khien Servo la i
Servo myservo2;
// Ta o doi tuong dieu khien Servo qua y ca m bien sieu a m
const byte MhdirectonL
;// Directon DC motor le羐
const byte MhdirectonR 7;// Directon DC motor right
const byte MhLhpin 6;
// PWM DC motor Le羐
const byte MhRhpin 5;
// PWM DC motor Right
const byte Shla ihpin 9; // Servo la i
const byte Shcbsa hpin 10; // Servo qua y ca m bien sieu a m
const byte trig 12;
// Thiet la p cha n pha t xcung ca m bien sieu a m
const byte echo 13;
// Thiet la p cha n nha n xcung ca m bien sieu a m
const byte enhle羐 2;
// Rea d encoder le羐
const byte enhright 3; // Rea d encoder right
const byte lihsehle羐 6; // Rea d a nna log light sensor le羐
const byte lihsehright 7; // Rea d a nna log light sensor right
//----------------------------------------------------------------------------------------------unsigned long tg;
// Luu gia tri tra ve tu ca m bien sieu a m
unsigned int kc,dis;
// Luu gia tri khoa ng ca ch da tnh toa n tu bien tg (cm )
unsigned int enhL,enhR;
// Luu gia tri doc encoder le羐, right
unsigned int Steeringhba la nce,Steeringhlimit;// Luu gia tri goc la i ca n ba ng, gioi ha n goc qua y vong
unsigned int BWhra ngeL,BWhra ngeR;
// Gia tri pha n biet line den va tra ng
floa t Kp1L,Kp1R;
// He so P re tra i, pha i khi pha t hien line
floa t Kp2L,Kp2R;
// He so P gia m toc do 2 ba nh sa u khi re
floa t Kp3L,Kp3R;
// He so P gia m toc do ba nh xce qua y vong trong khi re (chuc na ng visa i )
byte vuot;
byte tg1;
byte P;
// Vuot chuong nga i va t khi bien na y 1
// Thoi gia n xca c dinh ta n so la y ma u ca m bien sieu a m
// Chon do xce tra i, pha i
byte demvuot;
// Bien cong don de kich hoa t vuot chuong nga i va t
byte xcn;
// Chon xcinha n tra i 0, pha i 1
byte txcn;
// Tra ng tha i den xcinha n 0 OFF, 1 ON
int cbL,cbR;
// Luu gia tri ADC ca m bien do line tra ve sa u khi chuyen doi
int hhspeed;
int tocdo;
// Thoi gia n xce ta ng toc len doc
// Toc do xce
int dem1,dem2,dem3;
// Bien dem dung thuong xcuyen
int da nger;
// Dem khoa ng ca ch nguy hiem quet ca m bien sieu a m khi va o ba i do
int pa ssed;
// Dem khoa ng ca ch quet ca m bien sieu a m khi vuot qua va t ca n
int checkhpoint;
// Xa c dinh vi tri xcua t pha t
//------------------------------------------------------------------------------------------------// Thiet la p dieu khien dong co DC L-R
void SethuphDCmotor(){
pinMode(MhdirectonL,OUTPUT);
// Digita l
pinMode(MhdirectonR,OUTPUT);
// Digita l
pinMode(MhLhpin,OUTPUT);
// PWM
pinMode(MhRhpin,OUTPUT);
// PWM
//TCCR0B TCCR0B & B11111000 | B00000010;// Thiet la p ta n so xcua t xcung PWM (pin 5,6 ) 7, kHz
Seria l.println("Setup DC motor OK");
}
// Thiet la p cha n ca m bien sieu a m
void Sethuphcbsa (){
pinMode(trig,OUTPUT);
pinMode(echo,INPUT);
Seria l.println("Setup ca m bien sieu a m OK");
}
// Thiet la p 2 dong co Servo
void Sethuphservo(){
myservo1.a ta ch(Shla ihpin); // Khoi ta o cha n xcua t tn hieu dieu khien servo la i
myservo2.a ta ch(Shcbsa hpin);// Khoi ta o cha n xcua t tn hieu dieu khien servo qua y ca m bien sieu a m
Seria l.println("Setup servo OK");
}
// Thiet la p encoder L-R
void Sethuphencoder(){
a ta chInterrupt(0,ct1,FALLING);// Khoi ta o cha n nga t ngoa i INT0 (pin 2 ) - Kenh A encoder le羐
a ta chInterrupt(1,ct2,FALLING);// Khoi ta o cha n nga t ngoa i INT1 (pin 3 ) - Kenh A encoder right
Seria l.println("Setup encoder OK");
}
// Thiet la p la i
void Sethuphsteering(){
Steeringhba la nce
1;// Goc la i o giua
Steeringhlimit 30; // Gioi ha n goc la i
Seria l.println("Setup steering OK");
}
// Thiet la p ca m bien hong ngoa i do line
void Sethuphlinehdriving(){
//Bla ckhle羐 43;Whitehle羐 273; // Gia tri ADC gioi ha n line den, tra ng le羐 va right
//Bla ckhright 73;Whitehright 343; // Enviroment 9 0
Kp1L 0.5;Kp1R 0.5;
Kp2L 0.15;Kp2R 0.15;
// Da t gia tri ha ng so Kp bo dieu khien goc la i khi do line le羐 va right
// Da t gia tri ha ng so Kp bo dieu khien gia m toc do xce khi va o cua
Kp3L 0.5;Kp3R 0.5;
// Visa i
BWhra ngeL 1 3;BWhra ngeR 133;
// Da t gia tri moc pha n biet line den, tra ng le羐 va right
Seria l.println("Setup line driving OK");
}
// Thiet la p den
void Sethuphlight(){
pinMode(lihhea d,OUTPUT);
pinMode(lihrea r,OUTPUT);
pinMode(lihL,OUTPUT);
pinMode(lihR,OUTPUT);
Seria l.println("Setup light OK");
}
//----------------------------------------------------------------------------------------------// Chuong trinh phuc vu nga t ngoa i INT0 (encoder le羐 )
void ct1(){enhL++;}
// Chuong trinh phuc vu nga t ngoa i INT1 (encoder right )
void ct2(){enhR++;}
//----------------------------------------------/* Chuong trinh dieu khien xce ten lui Ver 1.0 - 17/04/2017
Shdirectonn: Huong qua y vongn: LEFT (3), RIGHT (4)
Sha nglen: Goc qua y vong (0-Steeringhlimit)
Movehdirectonn: Chieu chuyen dong cua xcen: FORWARD (1), BACKWARD (2)
Le羐hspeedn: Toc do ba nh xce ben tra i (0-255)
Righthspeedn: Toc do ba nh xce ben pha i (0-255) */
void Ca rhmoving(byte Shdirecton,byte Sha ngle,byte Movehdirecton,byte le羐hspeed,byte righthspeed){
if(Movehdirecton 1) {
digita lWrite(MhdirectonL,0);
digita lWrite(MhdirectonR,0);
a na logWrite(MhLhpin,le羐hspeed);
a na logWrite(MhRhpin,righthspeed);
}
if(Movehdirecton 2) {
digita lWrite(MhdirectonL,1);
digita lWrite(MhdirectonR,1);
a na logWrite(MhLhpin,le羐hspeed);
a na logWrite(MhRhpin,righthspeed);
}
if(Sha ngle>Steeringhlimit) Sha ngle Steeringhlimit;
if(Shdirecton 4) myservo1.write(Steeringhba la nce-Sha ngle);
if(Shdirecton 3) myservo1.write(Steeringhba la nce+Sha ngle); //+6
}
//----------------------------------------------// Chuong trinh tnh khoa ng ca ch ung voi goc qua y servo ca m bien sieu a m
// a nglen: 0-1 0
unsigned int Dista nce(byte a ngle){
myservo2.write(a ngle);
// Qua y servo toi goc a ngle
dela y(tg1);
digita lWrite(trig,0);
dela yMicroseconds(2);
digita lWrite(trig,1);
dela yMicroseconds(5);
digita lWrite(trig,0);
tg pulseIn(echo,HIGH,20000);// Do thoi gia n pha n hoi song sieu a m
kc int(tg/2/29.412);
// Tinh khoa ng ca ch
return kc;
}
//----------------------------------------------// Chuong trinh do line tra i Ver 1.0 - 17/04/2017
// Toc do xcen: speedh 0 - 255
void Lline(byte speedh){
int a ngleh3;
BWhra ngeL 153;
bien do line tra i
// Dieu chinh gia tri pha n biet line tra ng ca m
cbL ma p(a na logRea d(lihsehle羐),0,1023,1023,0);
if(cbL>BWhra ngeL) {
// Doc ADC ca m bien do line tra i
// Neu line tra ng
a ngleh3 (cbL-BWhra ngeL)*0.01;if(a ngleh3>Steeringhlimit) a ngleh3 Steeringhlimit;// Tinh goc re pha i
Ca rhmoving(RIGHT,a ngleh3,FORWARD,speedh,speedh);}
// Xe re pha i
else{
a ngleh3 (cbL-BWhra ngeL)*0.01;if(a ngleh3>Steeringhlimit) a ngleh3 Steeringhlimit;// Tinh goc re tra i
Ca rhmoving(LEFT,a ngleh3,FORWARD,speedh,speedh);
// Xe re tra i
}
dela y(2);
}
//----------------------------------------------// Chuong trinh do line pha i Ver 1.0 - 17/04/2017
// Toc do xcen: speedh 0 - 255
void Rline(byte speedh){
int a ngleh3;
cbR ma p(a na logRea d(lihsehright),0,1023,1023,0);
if(cbR>BWhra ngeR) {
// Doc ADC ca m bien do line pha i
// Neu line tra ng
a ngleh3 (cbR-BWhra ngeR)*0.05;if(a ngleh3>Steeringhlimit) a ngleh3 Steeringhlimit;// Tinh goc re tra i
Ca rhmoving(LEFT,a ngleh3,FORWARD,speedh,speedh);}
// Xe re tra i
else{
a ngleh3 (BWhra ngeR-cbR)*0.05;if(a ngleh3>Steeringhlimit) a ngleh3 Steeringhlimit;// Tinh goc re
pha i
Ca rhmoving(RIGHT,a ngleh3,FORWARD,speedh,speedh);
}
// Xe re pha i
dela y(2);
}
//----------------------------------------------// Chuong trinh tu do xce Ver 1.0 - 17/04/2017
void AutoP(){
BWhra ngeL 15 ;
// Dieu chinh gia tri pha n biet line tra ng ca m bien do line tra i
tg1 0;dem2 0;da nger 0;enhL 0;
while(enhL<55) {
// Reset ca c bien ve 0
// Xe doc encoder cha y den cuoi ba i do
Lline( 0);
// Xe cha y do line tra i va o ba i do
dem2++;
// Xa c dinh thoi diem doc ca m bien sieu a m (dem2 10)
if(enhL>20 && dem2>10 && enhL<40){
dem2 0;
dis Dista nce(16);
// Qua y ca m bien sieu a m sa ng pha i do khoa ng ca ch
if(dis<30 && dis>3) da nger++; // Co va t ca n
}
}
Ca rhmoving(RIGHT,0,FORWARD,0,0); // Xe dung o va ch cuoi ba i do
dela y(300);
if(da nger> ) P 2;else P 1;
if(P 1){
// Chon truong hop do xce dua va o bien da nger
// Do xce ben pha i
digita lWrite(lihrea r,1);
// Ba t den sa u
Ca rhmoving(LEFT,20,BACKWARD, 0, 0);// Lui xce
xcn 1;
// Ba t xcinha n tra i
Ca rhdis(LEFT,35);
xcn 0;digita lWrite(lihL,0);
digita lWrite(lihrea r,0);
// Doc encoder cha y 1 doa n
// Ta t xcinha n
// Ta t den sa u
Ca rhmoving(RIGHT,27,FORWARD, 0, 0);// Xe re pha i va o vi tri do
xcn 2;// Ba t xcinha n pha i
Ca rhdis(LEFT,35);
// Doc encoder cha y 1 doa n
xcn 0;digita lWrite(lihR,0);
// Ta t xcinha n
Ca rhmoving(RIGHT,0,FORWARD,0,0); // Dung xce
digita lWrite(lihhea d,0);
// Ta t den truoc
digita lWrite(lihhea d,0);
while(1){};
// La p vo ha n
}
if(P 2){
// Do xce ben tra i
digita lWrite(lihrea r,1);
// Ba t den sa u
Ca rhmoving(LEFT,5,BACKWARD, 0, 0);// Lui xce
Ca rhdis(LEFT,51);
// Doc encoder cha y 1 doa n
digita lWrite(lihrea r,0);
// Ta t den sa u
Steeringhlimit 37;
// Ta ng gioi ha n goc la i
Ca rhmoving(LEFT,37,FORWARD, 0, 0);// Xe re tra i va o vi tri do
xcn 1;
// Ba t xcinha n tra i
Ca rhdis(LEFT,75);
// Doc encoder cha y 1 doa n
xcn 0;digita lWrite(lihL,0);
// Ta t xcinha n
Ca rhmoving(RIGHT,0,FORWARD,0,0); // Dung xce
digita lWrite(lihhea d,0);
// Ta t den truoc
digita lWrite(lihhea d,0);
while(1){};
// La p vo ha n
}
}
//----------------------------------------------// Chuong trinh dieu khien xce do line cha y tu dong Ver 1.0 - 17/04/2017
void Linehdriving(byte speedh ){
int speedh2,a ngleh2,a ngleh3;
cbL ma p(a na logRea d(lihsehle羐),0,1023,1023,0);
cbR ma p(a na logRea d(lihsehright),0,1023,1023,0);
speedh2 speedh;
// Doc ADC ca m bien do line tra i
// Doc ADC ca m bien do line pha i
if(cbL>BWhra ngeL) {
// Ca m bien ben tra i nha n line tra ng
a ngleh3 (cbL-BWhra ngeL)*Kp1L;if(a ngleh3>Steeringhlimit) a ngleh3 Steeringhlimit;// Tinh goc re
pha i
//speedh2 speedh-(cbL-BWhra ngeL)*Kp2L;
// Gia m toc do xce khi re tra i, pha i
Ca rhmoving(RIGHT,a ngleh3,FORWARD,speedh2,speedh2-(cbL-BWhra ngeL)*Kp3L);}
if(cbR>BWhra ngeR) {
// Xe re pha i
// Ca m bien ben pha i nha n line tra ng
a ngleh2 (cbR-BWhra ngeR)*Kp1R;if(a ngleh2>Steeringhlimit) a ngleh2 Steeringhlimit;// Tinh goc re
tra i
//speedh2 speedh-(cbR-BWhra ngeR)*Kp2R;
// Gia m toc do xce khi re tra i, pha i
Ca rhmoving(LEFT,a ngleh2,FORWARD,speedh2-(cbR-BWhra ngeR)*Kp3R,speedh2);}
if(cbL< BWhra ngeL && cbR< BWhra ngeR){
// 2 ca m bien nha n line den
Ca rhmoving(RIGHT,0,FORWARD,speedh2,speedh2);
// Xe cha y tha ng
}
if(tocdo 200) {
hhspeed++;
// Dem thoi gia n xce ta ng toc len doc
if(hhspeed>1000){
hhspeed 0;
tocdo 120;
}}
dela y(2);
}
//----------------------------------------------// Chuong trinh xce cha y theo qua ng duong Ver 1.0 - 17/04/2017
// enh n: chon encoder LEFT (3), RIGHT (4)
// dis1 n: qua ng duong ca n di chuyen (cm) tnh theo duong tha ng
void Ca rhdis(byte enh,int dis1){
int en1,demxcn;
if(enh 3){
en1 dis1*100/ 4;
// Xe re tra i
// Tinh so xcung encoder theo qua ng duong
enhL 0;demxcn 0;txcn 0;
while(enhL30000) {txcn ~txcn;digita lWrite(lihL,txcn);}// Chop ta t xcinha n tra i
if(xcn 2 && demxcn>30000) {txcn ~txcn;digita lWrite(lihR,txcn);}// Chop ta t xcinha n pha i
if(demxcn>30000) demxcn 0;
dela yMicroseconds(1);}
}
if(enh 4){
en1 dis1*100/129;
// Tinh so xcung encoder theo qua ng duong
enhR 0;
while(enhR300) dis Dista nce(16);
pha i 90 do
// Do khoa ng ca ch khi ca m bien sieu a m qua y sa ng
//if(enhL>47) pa ssed 31;
// Doc encoder uoc luong khoa ng ca ch vuot qua va t ca n
if(dis>20) pa ssed++;
// pa ssed cong don khi xce vuot qua va t ca n
}
Ca rhmoving(RIGHT,15,FORWARD,tocdo-50,tocdo-50);
xcn 2;
// Xe re pha i
// Ba t xcinha n pha i
Ca rhdis(LEFT,20);
// Doc encoder cha y 1 doa n
xcn 0;digita lWrite(lihR,0);
// Ta t xcinha n pha i
while( ma p(a na logRea d(lihsehright),0,1023,1023,0)950 && checkhpoint< 1023){// Xe xcua t pha t o vi tri Sta rt
dis Dista nce(104);
// Qua y ca m bien sieu a m tha ng phia truoc
Ca rhmoving(RIGHT,0,FORWARD,tocdo,tocdo); // Xe cha y tha ng
dela y(500);
digita lWrite(lihhea d,1);
// Thoi gia n xca c dinh 2 ca m bien do line vuot qua va ch xcua t pha t
// Ba t den truoc
while(ma p(a na logRea d(lihsehle羐),0,1023,1023,0)<250 ||
ma p(a na logRea d(lihsehright),0,1023,1023,0)<310){Rline(tocdo);}// Xe cha y do la i pha i den khi 2 ca m
bien pha t hien line nga ng
Steeringhlimit 34;
// Ta ng gioi ha n goc la i
Ca rhmoving(LEFT,34,FORWARD, 0, 0);// Xe re tra i
xcn 1;
// Ba t xcinha n tra i
Ca rhdis(LEFT,72);
xcn 0;digita lWrite(lihL,0);
// Doc encoder cha y 1 doa n
// Ta t xcinha n tra i
Steeringhlimit 30;
digita lWrite(lihrea r,1);
// Ba t den sa u
BWhra ngeR 200;
// Gia m do nha y ca m bien do line pha i
for(dem3 1;dem3< 700;dem3++) Linehdriving(50);// Xe gia m toc do, cha y do line 2 ben xcuong doc
BWhra ngeR 133;
// Ta ng do nha y ca m bien do line pha i ve binh thuong
digita lWrite(lihrea r,0);
// Ta t den sa u
checkhpoint
// Da t vi tri xcua t pha t tep theo
50;}
if(checkhpoint> 00 && checkhpoint<900){
digita lWrite(lihhea d,1);
// Xe xcua t pha t o vi tri checkpoint 1
// Ba t den truoc
while(vuot<1) {
// La p den khi xce pha t hien va t ca n
if(vuot<1) dis Dista nce(104);
// Do khoa ng ca ch phia truoc xce
if(dis<54 && dis>49 && vuot<1) demvuot++; // Gioi ha n khoa ng ca ch pha t hien va t ca n
if(demvuot>2) vuot 1;
Linehdriving(tocdo);}
Vuot1();
checkhpoint 650;}
// Xa c dinh co va t ca n
// Xe cha y do line 2 ben
// Thuc hien chuong trinh vuot chuong nga i va t 1
// Da t vi tri xcua t pha t tep theo
if(checkhpoint>600 && checkhpoint<700){
digita lWrite(lihhea d,1);
dis Dista nce(104);
// Xe xcua t pha t o vi tri checkpoint 2
// Ba t den truoc
// Qua y ca m bien sieu a m tha ng phia truoc
dem3 0;
while(1 1){
// La p den khi xce pha t hien va ch nga ng tren doc
if(tocdo 200) dem3++;
if(ma p(a na logRea d(lihsehle羐),0,1023,1023,0)>BWhra ngeL &&
ma p(a na logRea d(lihsehright),0,1023,1023,0)>BWhra ngeR) tocdo 200;// Xe ta ng toc khi 2 ca m bien do
line pha t hien va ch nga ng duoi cha n doc
Linehdriving(tocdo);// Xe cha y do line 2 ben
if(ma p(a na logRea d(lihsehle羐),0,1023,1023,0)>100 &&
ma p(a na logRea d(lihsehright),0,1023,1023,0)>120 && dem3>50) brea k;// Thoa t khoi vong la p khi 2 ca m
bien do line pha t hien va ch nga ng tren doc
}
Steeringhlimit 35;
// Ta ng gioi ha n goc la i
Ca rhmoving(LEFT,33,FORWARD,tocdo-30,tocdo-10);// Xe gia m toc do, re tra i va o ba i do
xcn 1;
Ca rhdis(LEFT,60);
xcn 0;digita lWrite(lihL,0);
// Ba t xcinha n tra i
// Doc encoder cha y 1 doa n
// Ta t xcinha n tra i
Steeringhlimit 30;
BWhra ngeL 153;
// Ta ng do nha y ca m bien do line tra i
while(ma p(a na logRea d(lihsehright),0,1023,1023,0)<20 ||
ma p(a na logRea d(lihsehle羐),0,1023,1023,0)<15 ) Lline( 5);// Xe do line tra i cha y va o ba i do den khi 2
ca m bien do line pha t hien va ch nga ng truoc ba i do
AutoP();}// Thuc hien chuong trinh tu dong do xce
}
//------------------------------------------------------------------------------------------------// CHUONG TRINH CHINH ///////////////////////
void setup() {
Seria l.begin(9600);// Khoi ta o gia o tep seria l voi ma y tnh de xcua t ca c du lieu ca n thiet
Seria l.println("---iCa r contest controler progra ming---");
SethuphDCmotor();
Sethuphcbsa ();
Sethuphservo();
Sethuphencoder();
Sethuphsteering();
Sethuphlinehdriving();
Sethuphlight();
//dela y(10000);
enhL 0;enhR 0;
vuot 0;hhspeed 0;tocdo 110;tg1 20;pa ssed 0;demvuot 0;xcn 0;
}
void loop() {
checkhpoint a na logRea d(A5);// Doc nut nha n xca c dinh vi tri xcua t pha t
checkhpoint a na logRea d(A5);
checkhpoint a na logRea d(A5);
Sta rt();// Thuc hien chuong trinh chinh dieu khien xce
}
- Xem thêm -
.DOCX 
14 
15 
93 
