- Hiện thực hàm int laneright(int speed)
7. Hàm test() dùng để test các bộ phận xe
- Tham khảo code hàm void test() trong file test.c (thư mục test).
Các mode chính:
- Case 0: (chọn swicth trên board vi điều khiển). Mode này dùng để lấy mẫu màu đường đua, giúp xe có thể phân biệt màu trắng đen trên đường đua. Cách thực hiện như sau: bật nguồn Mạch VĐK, gạt DipSW trên mạch VĐK sang mode 0 (led 7 đoạn hiển thị giá trị 00), đặt Mạch Dò Đường trong vùng line trắng, nhấn bất kỳ nút nhấn nào trong 4 nút trên Mạch VĐK thì xe tự động cập nhật giá trị adc của màu trắng, led 7 đoạn sẽ thay đổi liên tục trong khi cập nhật ADC, khi led 7 đoạn hiện lại giá trị 00 thì việc cập nhật ADC hoàn tất. Sau đó ta quét Mạch Dò Đường ở các vùng trắng và đen xem các led hiển thị xem xe có nhận được màu đen không, nếu không ta có thể chỉnh lại mức tính adc compare trong hàm update_vcompare(). - Case 1: dùng để test 2 động cơ bánh,
Nhấn K0 động cơ trái quay lùi Nhấn K1 động cơ trái quay tiến Nhấn K2 động cơ phải quay lùi Nnhấn K3 động cơ phải quay tiến
Nếu các động cơ không chạy theo đúng chiều như vậy thì tốt nhất nên đảo các jack cắm nếu đảo lộn động cơ trái và phải, hay đảo dây đỏ đen của zack
cắm nếu động cơ không quay đúng chiều nêu trên, không nên chỉnh sửa chương trình.
- Case 2: dùng để test và chỉnh động cơ rc servo ***Test
Nhấn K0 servo bẻ sang phải Nhấn K3 servo bẻ sang trái.
Nếu servo không bẻ đúng chiều như trên thì nên chỉnh sửa lại cách tính góc trong hàm handle (). Cụ thể là sửa dấu cộng thành trừ hoặc ngược lại. Chú ý có khi hai servo cùng loại cùng nhãn hiệu nhưng lại ngược chiều quay là chuyện bình thường.
***Chỉnh
Khi bật nguồn (cả Mạch VĐK và Mạch Công Suất) mà thấy đầu xe không ở vị trí 0 độ thì cần phải chỉnh lại, thao tác như sau:
-Chuyển sang mode 2 (case 2). -Bật nguồn
-Dùng nút K0 và K3 chỉnh lại cho đầu xe ngay chính giữa (vị trí 0 độ). -Nhấn K2 để cập nhật.
-Tắt nguồn và bật lại để kiểm tra.
Xây dựng giải thuật điều khiển xe 1. Các hàm cơ bản của chương trình void speed(int left, int right);
Điều khiển duty cycle cho hai bánh phát động bên trái và phải. Giá trị truyền vào left và right là từ -255 đến 255.
void handle(int angle);
Điều chỉnh góc của động cơ servo so với phương vuông góc với thân xe. Đơn vị của đối số angle là độ ( 0 ). Xem hình 3.1
Posted Image
unsigned char sensor_inp(unsigned char MASK);
Hàm trả về giá trị của 8 sensor dò trước đầu xe sau khi AND với MASK. Ví dụ:
Posted Image
2. Cấu trúc chương trình
Chương trình chia làm 4 trạng thái chính: Posted Image
•Bước chuyển (1: I -> II) Khi thấy nửa line bên trái:
Posted Image
•Bước chuyển (2: II -> I)
Khi đã đi qua được đoạn đường chuyển làn trái. Posted Image
•Bước chuyển (3: I -> III) Khi thấy được nửa line bên phải. •Bước chuyển (4: III -> I)
Khi chạy qua được đoạn chuyển làn phải.
•Bước chuyển (5: I -> IV), (7: II -> IV), (8: III -> IV) Ngay khi thấy được nguyên một line.
Posted Image
•Bước chuyển (6: IV -> I)
Khi chạy xong được đoạn đường cua 90 độ. ***Chú ý:
* Ưu điểm của sơ đồ giải thuật là giải quyết được lỗi nhầm lẫn giữa đoạn đường cua vuông thành đoạn đường chuyển làn. Thường thì không phải lúc nào mạch dò cũng song song với vạch trắng ngang, có khi nữa bên mạch dò bắt được vạch trắng trước ( hình 5.7 ) sinh ra lỗi nhầm lẫn đoạn đường cua vuông thành chuyễn làn, việc này thường xuyên xảy ra.
* Khi mạch dò đi ngang qua nữa vạch trắng mà mạch dò nằm lệch qua phía nữa vạch trắng thì có thể bị nhầm lẫn đoạn đường chuyển làn thành đoạn cua vuông ( hình 5.8 ), nhưng thường thì ở đường đua thật đúng chuẩn qui định thì lỗi này ít xảy ra. Lỗi này gặp khi sau đường cong là hai nữa vạch trắng chuyển làn xe chưa kịp chỉnh mạch dò vào giữa đường đua. Khi xảy ra lỗi này cũng không đáng ngại vì với kỹ thuật qua đoạn cua vuông thì xe cũng dễ dàng qua được đoạn đường chuyển làn chỉ là khi này vận tốc xe chậm hơn so với khi không nhầm lẫn thôi.
Posted Image Posted Image
3. Giải thuật xử lý khi qua các đoạn đường thẳng và cong
- Các trạng thái sensor có thể gặp thể hiện tương đối độ lệch hướng của xe so với phương đường đua:
Posted Image
- Cách tính tỉ lệ vận tốc hai bánh sau theo góc bẻ lái của hai bánh trước: Posted Image
Từ hình vẽ ta có: Posted Image
Từ (1), (2), (3) suy ra tỉ lệ vận tốc hai bánh theo góc bẻ lái: Posted Image Ví dụ: T = 0,14 m W = 0,175 m Ta có bảng sau: Posted Image
- Sơ đồ giải thuật của trạng thái chính I ( xem hình 3-11): Posted Image
- Ý tưởng khi qua các đoạn đường thẳng và cong:
Ta dựa vào trạng thái sensor để biết được độ lệch hướng của chạy của xe so với phương của đường đua, ứng với một trạng thái sensor ta chọn một góc bẻ lái hợp lý ( bằng cách dùng hàm handle (int) ) sao cho xe có xu hướng di chuyển về phía chính giữa đường đua, với độ lệch hướng càng lớn thì ta chọn góc bẻ lái càng lớn.
Posted Image
Ưu điểm của cách chạy này là: xe chạy qua các đường cong với bán kính cố định rất nhanh giống như đang chạy trên đường thẳng khi xe đã vào được đường cong. Thực tế thì trong các cuộc thi MCR chỉ có hai loại đường cong với hai bán kính khác nhau. Nên ta chỉ cần chọn hai góc bẻ lái hợp lý ứng với hai độ lệch hướng khác nhau để qua 2 loại đường cong sao cho hai độ lêch hướng được chọn là hai độ lệch hướng trung bình – cụ thể là độ lệch hướng ứng với biến old_state = +/-3 và old_state = +/-4 (xem hình 5.9 ở trên). Tại mỗi khúc chuyển giao giữa đường thẳng sang đường cong và giữa đường cong sang đường cong hướng khác ta tắt 2 động cơ phát động để toàn bộ nguồn cung cấp cho rc servo bẻ lái ( thực tế chỉ trong 20 – 40 ms ) để xe có thể chạy ở vận tốc ổn định cao hơn bình thường. Và khi phát hiện xe lệch ra khỏi phương chính giữa đường đua quá xa cũng là khi mạch dò bắt được cả line giữa và line biên ta cho xe hãm tốc mạnh để tránh trường hợp xe văng ra khỏi đường đua.
Posted Image
- Ý tưởng trên được hiện thực trong hàm runforwardline (int _speed) - Biến old_state dùng để lưu lại trạng thái sensor trước đó, dựa vào đó ta tránh trường hợp xe bắt nhằm một trong hai line biên.
- Biến brake_flag, hàm brake(int time), hàm brake_timer (int time, int_ speed) dùng để hiện thực ý tưởng cho xe hãm tốc mỗi khi qua đoạn chuyển giao
đường thẳng thành đường cong hay đường cong thành đường cong khác hướng.
- Hàm brake (int time) là hàm hãm tốc bằng cách cho vận tốc hai bánh bằng 0 (speed (0,0) ) mà vẫn điều khiển bẻ lái servo cho xe hướng theo line tương tự như hàm runforwardline ( ).
- Hàm brake_timer (int time, int_ speed ) tương tự như hàm brake ( int time ) nhưng ta có thể chỉnh vận tốc hai bánh bằng thông số _speed truyền vào hàm. Hàm dùng để hãm tốc mạnh.
4. Sơ đồ trạng thái khi qua đoạn đường cua vuông - Thứ tự các sự kiện khi qua đoạn cua vuông: Posted Image
- Sơ đồ trạng thái: Posted Image
- Cho xe chạy đến khi toàn bộ sensor đều nhận được màu đen rồi mới bẻ cua giúp cho xe tránh được lỗi quẹo sớm ngay khi gặp hai vạch trắng báo hiệu. - Hiện thực hàm int turn90(int _speed)
5. Sơ đồ trạng thái khi qua chuyển làn phải - Thứ tự các sự kiện khi qua chuyển làn phải: - Sơ đồ trạng thái:
Posted Image - Sơ đồ trạng thái: Posted Image
- Hiện thực hàm int laneright(int _speed) 6. Sơ đồ trạng thái khi qua chuyển làn trái - Tương tự như chuyển làn phải.
- Hiện thực hàm int laneleft(int _speed) source:bkit4u
Viết code cho robot dò đường
Mình mới tập làm robot đơn giản. Sưu tầm trên mạng được 1 mạch đơn giản như sau, dò đường theo vạch đen chạy trong đường cong kín
Mình rút gọn lại chỉ sử dụng 3 cặp cảm biến dùng quang trở. sắp xếp hàng ngang, 1 cặp nằm trong vạch đen, 2 cặp nằm ngoài vạch đen
Sử dụng chân 2,3,6 cho ngõ vào cảm biến vi đk.
Mạch đã làm xong, nhưng còn phần mềm thì mình ko rành. Các bạn có thể viết mẫu cho mình chương trình theo như phần cứng mình đã nói trên không? Mình đã cài Keil C rồi. Có chú thích thì càng tốt.
Theo mình thì thế này : bình thường khi quang trở ở giữa ko nhận đc ánh sáng và 2 quang trở bên ngoài line đen nhận đc as thì robot chạy thẳng, 2 motor quay cùng tốc độ. Khi quang trở bên trái và ở giữa cùng nhận đc as và quang trở bên phải ko nhận đc ánh sáng thì cho motor bên trái quay nhanh, bên phải quay chậm để quẹo phải... vâng vâng. Nhưng ...ko biết viết thế nào .
À. mình dùng băng keo đen bề rộng là 5cm làm line cho roboot chạy theo. Cái này mình có làm rồi,nhưng mình dùng 4 cặp led thu phát đưa tín hiệu vào AT89C51.đề điều chỉnh tốc độ động cơ thì bạn sử dụng phương pháp PWM.nếu
cần mình sẽ post lên cho bạn xem thử.thân !
Tham gia
Mình có tìm được đoạn code này, tuy nhiên ko hiểu chỗ khai báo cho 2 biến tốc độ trái, phải. Đây là code cho mạch trên. Nếu ko dùng PWM thì làm sao để chỉnh đc tốc độ nhỉ?
#include d:\mc51\8051io.h #include d:\mc51\8051reg.h
extern register unsigned char speedleft,speedright; register unsigned char high,low,flag,time;
main() { P1=0x40; P3=0xff; high = 80; low = 30; flag = 0; time = 50; Start(); while(1) { P3|= 0x0f; Run(); } } Start() { char exit,key; exit =1; while(exit) { key = P1;
if((key & 0x40)==0) exit=0; } } Run() { char sensors; sensors = (P3 &=0x0f); if((sensors & 0x01)==0) { TurnRight(); flag = 1; }
else if((sensors & 0x08)==0) { TurnLeft(); flag = 2; } else if(sensors == 0x09) { Forward(high); flag = 0; } else if(((sensors==0x0b)||(sensors==0x0d))&&(flag==0)) Forward(low); } Forward(char speed) { P1=0x64; speedright = speed+10; speedleft = speed;
delay(time); } TurnRight() { P1=0x68; speedright = low+5; speedleft = low; delay(time); } TurnLeft() { P1=0x54; speedright = low+5; speedleft = low; delay(time); } Reverse(char speed) { P1=0x58; speedright = speed; speedleft = speed+5; delay(time); }
Bạn đã chạy thử xe bằng chương trình trên chưa?Mình nghĩ chương trình này chỉ chạy xe được ở tốc độ rất chậm thôi,và khó có thể chạy theo đường cong bất kì ( cua gấp,gặp ngã 3 ...)
Mình có 1 số vần đề lưu ý bạn khi làm xe như sau :
Về cơ khí : quan trọng nhất là 2 trục bánh phải thật thẳng,bánh không được đảo hay xăng vành vì nếu thế thì việc bạn chạy theo đường thẳng đã rất khó rồi Ngoài ra bạn cũng nên chú ý vấn đề chống nhiễu cho sensor,ví dụ cả 3 cặp led cùng bắt đường ( do nền chẳng hạn ),khi đó xe sẽ chạy sai.
Để dò đương chuẩn hơn bạn nên dùng 4 hoặc 6 cặp thu phát
Hi,mình cũng làm bánh bằng bánh xe đồ chơi,hàn vào trục động cơ(cho thêm cả 502 cho chắc )nhưng vẫn không thể làm cho nó thẳng được.Về chương trình bạn nên tự viết để sau đó còn phát triển thêm nữa(cua gấp,gặp ngã 3,ngã tư,tránh vật cản...).Đây là chương trinh mình viết cho xe của mình,post cho bạn tham khảo nhé :
PHP Code: #include <REGX51.H> #define roletrai P1_1 #define rolephai P1_4 #define dctrai P1_3 #define dcphai P1_0 #define phai1 P2_3 #define trai1 P2_0 #define trai0 P2_1 #define phai0 P2_2 #define tien 1 #define lui 0
unsigned char dem1=0,dem2=0;
unsigned char xungphai,xungtrai;
void delayms(int ms) //ham tao tre khoang 1ms
{
int i,j;
for(i=0;i<ms;i++) for(j=0;j<85;j++){}
}
void khoitao(void) { EA=0; TMOD=0x02; TL0=0x9B; TH0=0x9B; ET0=1; EA=1; TR0=1; }
void timer0(void)interrupt 1
{
TR0=0;
TF0=0;
dem1++;
dem2++;
if(dem1<=xungphai) { P1_0=1; } else { P1_0=0; }
if(dem1==10) dem1=0; if(dem2<=xungtrai) { P1_3=1; } else { P1_3=0; }
if(dem2==10) dem2=0;
TR0=1; } }
void stop(void) {
xungphai=0; xungtrai=0; }
void main(void) { khoitao(); P2_0=1;P2_1=1;P2_2=1;P2_3=1; roletrai=1; rolephai=1; delayms(50); xungphai=9; xungtrai=9; delayms(500); while(1) {
while (P2_0==1&&P2_1==0&&P2_2==0&&P2_3==0) //lech trai nhieu nhat {
xungphai=8; xungtrai=4;
if (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0)//xe lech ra khoi duong { delayms(500); //loai tru nhieu
xungphai=10; xungtrai=3;
delayms(150); //quay xe vao trong 150 ms
while (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0) //van con o ngoai duong xungphai=6;
xungtrai=4; //tiep tuc quay vao
while ( P2_0==1&&P2_1==0) //trai 1 bat vach {
xungphai=6;
xungtrai=8; //tang toc banh trai de xe di thang }
} } }
while (P2_0==1&&P2_1==1&&P2_2==0&&P2_3==0) //lech trai vua {
xungtrai=7; }
while (P2_0==0&&P2_1==1&&P2_2==0&&P2_3==0) //lech trai it nhat {
xungphai=8; xungtrai=6; }
while (P2_0==0&&P2_1==1&&P2_2==1&&P2_3==0) // khong lech {
xungphai=7; xungtrai=7; }
while (P2_0==0&&P2_1==0&&P2_2==1&&P2_3==0) //lech phai it nhat {
xungphai=6; xungtrai=8; }
while (P2_0==0&&P2_1==0&&P2_2==1&&P2_3==1) // lech phai vua {
xungphai=7; xungtrai=8; }
while (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==1) //lech phai nhieu {
xungphai=4; xungtrai=8;
if (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0) //ra khoi duong {
delayms(500); // loai tru nhieu xungtrai=10;
xungphai=3;
delayms(150); //quay vao trong 150ms
while (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0) //van chua bat duong xungphai=4;xungtrai=6;
while ( P2_3==1&&P2_2==0) // phai 1 bat duong {
xungphai=8;xungtrai=6; // tang toc banh phai de xe di thang
} } } }
if (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0) // ra khoi vach trang { delayms(700); //chong nhieu
while (P2_0==0&&P2_1==0&&P2_2==0&&P2_3==0) { roletrai=0;
rolephai=0; //di lui delayms(50); xungphai=8; xungtrai=8; } }
if (trai1==1&&trai0==1&&phai0==1&&phai1==0) //gap nga ba ben trai {
delayms(100); // chong nhieu if (trai1==1&&trai0==1&&phai0==1&&phai1==0)
{
stop(); //dung xe delayms(500);
roletrai=0; //quay ngang rolephai=1;
delayms(50); xungtrai=8; xungphai=8; delayms(200);
while (trai1==0||trai0==0) //trai 1 hoac trai 0 van o ngoai vach {
delayms(100);
if (trai1==1&&trai0==1&&phai0==1&&phai1==0) { stop(); delayms(500); roletrai=1; delayms(50); xungtrai=8; xungphai=8;
delayms(300); }
} } } }
if (trai1==0&&trai0==1&&phai0==1&&phai1==1) //gap nga 3 ben phai {
delayms(100); //chong nhieu if (trai1==0&&trai0==1&&phai0==1&&phai1==1)
{
stop(); delayms(500); roletrai=1; rolephai=0;
delayms(50); //quay ngang xungtrai=8;
xungphai=8; delayms(200);
while (phai1==0||phai0==0) {
delayms(100);
if (trai1==0&&trai0==1&&phai0==1&&phai1==1) { stop(); delayms(500); rolephai=1; delayms(50); xungtrai=8; xungphai=8; delayms(300); } } } } } }