THUẬT TOÁN DÒ ĐƯỜNG
Mục đích của việc dò đường: giúp cho robot di chuyển trên các vạch trắng có sẳn trên sân thi đấu để đến các mục tiêu và ghi điểm. Việc dò đường hay còn gọi là dò line làm tăng độ chính xác khi di chuyển của robot.
Cách lập trình dò đường:
- Dựa vào sự thay đổi trạng thái trên các cảm biến dò đường. Cảm biến dò đường được giới thiệu rất kỹ trong các tài liệu đã post trên diễn đàn. Nhưng có thể khái quát như sau:
Một cảm biến của robot thường gồm một con phát và một con thu như hình dưới, khi cấp nguồn thì con phát phát sáng. Mỗi khi đi qua vạch trắng ánh sáng phát từ con phát sẻ phản xạ xuống vạch trắng lên con thu, con thu nhận ánh sáng sẻ thay đổi mức điện áp từ 0v tương ứng với mức 0 thành 5v tương ứng với mức 1. Còn khi ở trên sân có các màu tối ánh sáng không phản xạ lên nên con thu luôn ở mức 0. Như vậy vi điều khiển sẻ liên tuc kiểm tra mức trạng thái có trên mỗi cảm biến thu để xác định được robot có đang ở trên vạch trắng hay không và xác định được độ lệch tương đối của robot so với vạch trắng dựa vào cách bố trí các cảm biến.
- Từ các giá trị của cảm biến đưa về vi điều khiển sẻ điều khiển tốc độ các động cơ dò đường một cách phù hợp nhất để đưa robot chạy vào giửa đường line nhằm đảm bảo độ chính xác khi di chuyển.
* Dưới đây là một số hình ảnh minh hoạ cho thuật toán dò đường theo vạch trắng.
Robot ở trạng thái lý tưởng trong quá trình dò đường.
Hình Các trường hợp dò đường
a. Robot đang bị lệch phải, trường hợp này xử lý bằng cách nạp tốc độ bánh phải lớn tốc độ hơn bánh trái.
b. Robot đang bị lệch trái, trường hợp này xử lý bằng cách nạp tốc độ bánh trái lớn hơn tốc độ bánh phải.
Các bước để có một chương trình dò đường:
Xác định các trường hợp mà các cảm biến có thể nhận trong quá trình dò đường.
- Ứng với mỗi giá trị từ module cảm biến, xác định vị trí lệch của robot so với vạch trắng.
Ví dụ:
8 cảm biến được nối vào 8 chân của vi điều khiển ở cổng P0 của vi điều khiển theo trình bày ở hình dưới:
Nếu giá trị tại cổng P0 là 00011000 thì giá trị tương ứng của mắt trái 1 và mắt phải 1 bằng 1, tức là hai mắt giửa của robot nhận được vạch trắng và có mức điện áp bằng 1, do đó ta biết được là robot đang nằm ở chính giửa vạch trắng.
Nếu giá trị tại cổng P0 là 11000000 thì giá trị tương ứng của mắt trái 4 và mắt trái 3 bằng 1, tức là hai mắt ngoài cùng bên trái của robot nhận được vạch trắng và có mức điện áp bằng 1, do đó ta biết được là robot đang nằm lệch nhiều về phía bên phải của vạch trắng.
Tương tự, nếu giá trị tại cổng P0 là 00000011 thì giá trị tương ứng của mắt phải 4 và mắt phải 3 bằng 1, tức là hai mắt ngoài cùng bên phải của robot nhận được vạch trắng và có mức điện áp bằng 1, do đó ta biết được là robot đang nằm lệch nhiều về phía bên trái của vạch trắng.
Có khá nhiều các vị trí tương đối của cảm biến so với vạch trắng và với mỗi vị trí đó ta hoàn toàn có thể xác định độ lệch của robot trên vạch trắng và từ đó điều khiển tốc độ của hai động cơ dò đường cho phù hợp để đưa robot về chính giữa vạch trắng nhằm đảm bảo độ chính xác cho robot di chuyển tới mục tiêu.
- như vậy sau khi đã xác định được các trường hợp mà khi dò đường robot có thể gặp phải, ta nhận giá trị của cảm biến và so sánh với các trường hợp đã thiết lập, nếu các cảm biến đang rơi vào trường hợp nào thì ta điều khiển động cơ cho từng trường hợp đó.
Chương trình dò đường đơn giản như sau:
void do_duong(void)
{
while(1)
{
ACC=P0; // nhận giá trị cảm biến từ cổng P0 vào thanh ghi đa chức năng ACC
if(ACC==11000000) // MT4 và MT3 nhận -> lệch phải nhiều
{
RMOTOR=200; // bánh trái chạy chậm hơn bánh phải
LMOTOR=50; // để điều chỉnh robot vào giửa line
}
if(ACC==01100000) // MT3 và MT2 nhận -> lệch phải vừa
{
RMOTOR=200;
LMOTOR=100;
}
if(ACC==00110000) // MT2 và MT1 nhận -> lệch phải ít
{
RMOTOR=200;
LMOTOR=50;
}
if(ACC==00011000) // hai mắt giữa nhận -> không lệch
{
RMOTOR=200;
LMOTOR=200;
}
if(ACC==00001100) // MP1 và MP2 nhận -> lệch trái ít
{
RMOTOR=50;
LMOTOR=200;
}
if(ACC==00000110) // MP2 và MP3 nhận -> lệch trái vừa
{
RMOTOR=100;
LMOTOR=200;
}
if(ACC==00000011) // MP3 và MP4 nhận -> lệch trái nhiều
{
RMOTOR=150; // bánh phải chạy chậm hơn bánh trái
LMOTOR=200; // để điều chỉnh robot vào giửa line
}
}
}
Chương trình này chỉ chứa một số trường hợp đơn giản, vẫn còn khá nhiều trường hợp và nhiều giải thuật khác hay hơn, mình chỉ đưa ra một ví dụ đơn giản để các bạn có thể phát triển tối ưu hơn