Tổng hợp các chân trên của vi điều khiển nối với các chân trên các module
Bảng 4: Tổng hợp các chân trên của vi điều khiển nối với các chân trên các module Chân trên vi điều
khiển
Nhiệm vụ Chân trên các
Module
PC6 Tín hiệu kích cho
cảm biến siêu âm trái hoạt động.
PC7 Tín hiệu báo thời điểm nhận lại được sóng phản xạ của cảm biến siêu âm trái, gửi vào timer của vi điều khiển để tính ra khoảng cách đến vật cản.
ECHO – Cảm biến trái
PC8 Tương tự chân PC6 TRIG – Cảm biến
phải
PC9 Tương tự chân PC7 ECHO – Cảm biến
phải
PA1 Vi điều khiển cấp
chuỗi xung PWM.
PWM – Driver phải
PD1 Vi điều khiển đưa
mức logic „High‟ hoặc „Low‟ để xác định chiều quay cho motor.
DIR – Driver phải
PA0 Vi điều khiển cấp
chuỗi xung PWM.
PWM – Driver trái
PD2 Vi điều khiển đưa
mức logic „High‟ hoặc „Low‟ để xác định chiều quay cho motor.
DIR – Driver trái
PD9 Chân truyền dữ liệu
của vi điều khiển
Rx – HC11
PD8 Chân nhận dữ liệu
của vi điều khiển
4.4. Mạch nguồn AC AC 220V 2 Driver của 2 động cơ DC Servo Nguồn 24V DC Mạch nguồn 5V DC Mạch nguồn LM2596
3 cảm biến siêu âm
STM32F4
Module RF Transceiver CC1101
2 Encoder của động cơ DC
Hình 3.27. Sơ đồ khối mạch nguồn cho Robot
Nguồn dùng cho mô hình:
Nguồn 24VDC cung cấp cho 2 driver của 2 động cơ DC Servo.
Nguồn 5V được sử dụng cho nhiều module và để cách ly giữa vi điều khiển và động cơ. Do đó, ta sử dụng nguồn 5V riêng từ bộ nguồn mà không chuyển từ bộ nguồn 24V xuống 5V.
Nguồn 5VDC cung cấp cho vi điều khiển, cảm biến siêu âm, Module RF, encoder của động cơ thông qua IC nguồn ổn áp LM2596.
Chƣơng 4:GIẢI THUẬT ĐIỀU KHIỂN Đặt vấn đề
4.1.
Giải thuật điều khiển gồm: giải thuật dò mê cung và giải thuật tìm đường đi ngắn nhất.
Giải thuật dò mê cung: có nhiệm vụ dò toàn bộ mê cung qua nhiều lần di chuyển của Robot đồng thời hiển thị mê cung đó trên máy tính.
Giải thuật tìm đường đi ngắn nhất: Khi robot hoàn thành dò toàn bộ mê cung. Khi đó sẽ có một đường đi tối ưu nhất từ vị trí vào đến vị trí ra. Giải thuật này giúp cho máy tính tính toán và tìm ra đường đi tối ưu này.
Yêu cầu giải thuật điều khiển:
Đảm bảo robot dò mê cung tìm được đường ra khỏi mê cung. Vẽ đầy đủ mô hình mê cung đã dựng.
Tìm đường di ngắn nhất khi đã dò toàn bộ mê cung.
Nguyên tắc xây dựng giải thuật điều khiển cho xe 4.2.
Dựa theo phân tích các thuật toán dò mê cung ở chương 2. Tác giả nhận thấy thuật toán Tremaux phù hợp với yêu cầu của đề tài.
Thuật toán này cho thấy tính hiệu quả khi do được toàn bộ mê cung theo phương pháp đánh dấu từng ô khi xe đi qua. Các thông tin đánh dấu gồm:
Hướng di chuyển của robot tại vị trí đánh dấu. Tọa độ vị trí đánh dấu.
Trạng thái bức tường xung quanh ô đánh dấu. Số lần robot qua vị trí ô đánh dấu.
Các thông tin này là cơ sở để xây dựng bản đồ mê cung khi robot hoàn thành các bước dò mê cung.
Mảng dữ liệu lƣu trữ 4.3.
4.3.1.Mảng dữ liệu lƣu trữ tức thời
Mảng dữ liệu này có nhiệm vụ lưu trữ thông tin trạng thái của từng ô được đánh dấu khi robot đi qua để tính toán vị trí robot cần đến tiếp theo.
Các qui ước lưu trữ
Hướng của robot tại ô đang xét:
Hướng Nam 1
Hướng Tây 2
Hướng Bắc 3
Hướng Đông 4
Trạng thái tường mê cung xung quanh robot tại ô đánh dấu dựa vào mã chuyển đổi theo bảng qui định sau:
Mã qui
đổi Trạng thái tường
Tường bên trái Robot Tường ở giữa Robot Tường bên phải Robot 0 Không có tường 0 0 0
1 Có tường bên phải 0 0 1
2 Có tường ở giữa 0 1 0
3 Có tường ở giữa và bên
phải 0 1 1
4 Có tường bên trái Robot 1 0 0
5 Có tường bên trái và bên
phải 1 0 1
6 Có tường bên trái và ở
giữa 1 1 0
7 Có tường cả ba hướng 1 1 1
Tọa độ vị trí đánh dấu: Tọa độ vị trí được xác định nhờ vào tọa độ vị trí trước đó và hướng di chuyển của robot.
Hướng X Y
1 X‟=X Y‟=Y-1
2 Y‟=Y X‟=X-1
3 Y‟=Y+1 X‟=X
4 Y‟=Y X‟=X+1
X‟ : tọa độ vị trí mới theo trục x X: tọa độ vị trí cũ theo trục x Y‟ : tọa độ vị trí mới theo trục y Y: tọa độ vị trí cũ theo trục y
4.3.2.Mảng dữ liệu lƣu trữ cố định
Mảng dữ liệu lưu trữ cố định có nhiệm vụ lưu trữ thông tin của tất cả các ô mà robot đi qua khi hoàn thành một lần dò tự vị trí vào đến vị trí ra. Dự liệu này được cập nhật cho lần di do tiếp theo để tránh trước hợp lăp ô không cần thiết. Khi robot dò toàn bộ mê cung, mảng dũ liệu này sẽ là cơ sở để thực hiển giải thuật tìm đường đi ngắn nhất.
Các mảng lưu trữ cố định gồm:
Mảng tọa độ vị trí robot từng đi qua theo file “toado.txt” theo qui ước (x,y)=(tọa độ x, tọa đô y).
Mảng chuyển đổi thành dữ liệu lưu trữ cho thuật toán tìm đường đi ngắn nhất theo file “data.txt” như sau:
Kích thước 8x32 để số hóa dữ liệu mê cung theo quy ước sau: mỗi ô (i, j) sẽ có 4 bit - bitBac, bitDong, bitNam, bitTay biểu thị cho tường xung quanh ô đó.
Bit = 0: hướng đi không có tường Bit = 1: hướng đi đã chạy qua một lần Bit = 2: hướng đi đã chạy qua 2 lần Bit = 3: hướng đi bị chắn bởi tường
Thông tin tường của ô (i, j) trên mảng đặt theo thứ tự sau:
Bảng 5 Bảng quy ước bit tường trong mảng
Ô (i, j) Cột thứ 4j – 3 Cột thứ 4j – 2 Cột thứ 4j - 1 Cột thứ 4j
Hàng thứ i -bitBac bitDong bitNam bitTay
Vậy bằng cách truy xuất các phần tử của mảng infoWall, tác giả sẽ số hóa, thay đổi, sử dụng được dữ liệu tường của mê cung.
Ví dụ: Phần tử Ô (2,8) chính là bit Tay của ô(2,2)
Lƣu đồ giải thuật 4.4.
4.4.1.Giải thuật điều khiển trên vi điều khiển 4.4.1.1 Chƣơng trình chính Cập nhật trạng thái cảm biến Lựa chọn hướng di chuyển Di thẳng Xoay phải Xoay trái Gửi dữ liệu lên máy tính
Hình 4.1. Sơ đồ điều khiển chương trình chính
Chương trình chính được trình bày trong bản vẽ Lưu đồ giải thuật. Chương trình chính có nhiệm vụ điều khiển xe di chuyển trong mê cung, đồng thời gửi dữ liệu lên máy tính để vẽ lại mê cung khi xe dò được. Cấu trúc của chương trình chính như sau:
Bước 2: Lựa chọn hướng di chuyển tiếp theo (xoay trái, đi thẳng, xoay phải). Bước 3: Gọi các hàm con Turn left, Turn right, Run Straight theo hướng được lựa chọn.
Bước 4: Cập nhật tọa độ vị trí mới theo hàm toadoketiep(Huong‟) nếu đi thẳng và hướng di chuyển nếu rẽ hướng.
Bước 5: Gửi dữ liệu hướng, tọa độ, cảm biến lên máy tính để vẽ bản đồ mê cung và lưu trữ thông tin,
Bước 6: lặp lại bước 1.
4.4.1.2 Giải thuật bám tƣờng để robot di chuyển từng ô mê cung
Run Straight
Cambien_giua>20 or encoder<600
Cambien_trai- cambien_phai<0
Bám tường bên trái
Bám tường bên phải Encoder=0 Dùng xe End Y N Y N
Hình 4.2. Giải thuật đi thẳng theo tường mê cung
Giải thuật giúp robot duy chuyển dọc tường theo một khoảng cách nhất định. Nhờ đó giúp robot di chuyển ổn định. Trong quá trình di chuyển, giải thuật điều khiển đồng thời tính số xung encoder và trạng thái cảm biến giữa để xác định robot ở vị trí ô tiếp theo hay chưa. Khi robot dừng tại vị trí ô tiếp theo, hệ thống cập nhật trạng thái các cảm biển để xác định hướng đi kế tiếp.
4.4.1.3 Giải thuật rẽ hƣớng
Turn Left
Set motor_left (250) Set motor_right(-250)
Huong =Huong -1; (if Huong <1 ) Huong=4;
End
Turn Right
Set motor_left (-250) Set motor_right(250)
Huong =Huong +1; (if Huong >4) Huong=1;
End
Hình 4.3. Giải thuật rẽ hướng của xe
Giải thuật rẽ hướng được thực hiện sau khi robot dừng tại ô kế tiếp. Trạng thái thông tin tại ô này giúp cho hệ thống quyết định robot di chuyển thẳng hay xoay sang trái hoặc sang phải. Giải thuật rẽ hướng giúp cho robot xoay chính xác một góc 900 sang trái hoặc sang phải phụ thuộc vào trạng thái cảm biến siêu âm tại ô đó. Khi robot xoay góc 900 đồng nghĩa với hướng của robot thay đổi. Biến “Huong” sẽ được điều chỉnh lại cho phù hợp với hướng mới của robot.
4.4.1.4 Giải thuật xác định tọa độ vị trí của robot
Giải thuật xác định tọa độ vị trí trí của xe nhằm cập nhật toạ độ chính xác của robot khi nó di chuyển đến ô kế tiếp. Giá trị tọa độ (x,y) thay đổi phù hợp với hướng di chuyển mới. Hướng vào tọa độ mới sẽ được cập nhập vào mảng lưu trữ tạm thời để làm cơ sở so sánh khi xây dựng thông tin trạng thái ô kế tiếp và xây dựng bản đồ mê cung tại ô đó.
Toadoketiep(Huong )
Huong =1 Huong =2 Huong =3 Huong =4
Y =Y-1 X =X Y =Y X =X-1 Y =Y+1 X =X Y =Y X =X+1 Return Toado(X,Y) N N N Y Y Y Y
Hình 4.4. Giải thuật xác định tọa độ của xe 4.4.2.Giải thuật quản lí dữ liệu và vẽ mê cung
4.4.2.1 Chƣơng trình chính
Sau khi robot dò đường đi và thu thập dữ liệu và truyền về cho máy tính, phần mềm Matlab vẽ mê cung dựa trên dữ liệu được lưu trên file data_old.txt và dữ liệu nhận về gồm hướng, tọa độ x,y và Status_sensor. Cuối cùng là lưu dữ liệu vừa nhận được vào file data_old.txt để sử dụng cho những lần tiếp theo.
Start Receive data RS232 Giai ma du lieu (huong,x,y,sensor) Cobao=1 Ve me cung Save data.txt Cobao=0 X=10 &y=10 Load data_old.txt Ve me cung theo du lieu End Ve me cung N Y
Hình 4.5. Chương trình chính trên matlab 4.4.2.2 Giải thuật vẽ mê cung
Giải thuật vẽ mê cung có nhiệm vụ nhận thông tin hướng, tọa độ, trạng thái cảm biến từ vi điều khiển để vẽ chính xác bản đồ mê cung.
Ve me cung Huong ==1 Huong==2 Huong == 4 Huong ==3 Sensor Left==1 Sesor Mid==1 Sensor Right==1 Plot ([x+0.5 x+0.5],[y- 0.5 y+0.5]) Plot ([x-0.5 x+0.5],[y- 0.5 y-0.5]) Plot ([x-0.5 x-0.5],[y- 0.5 y+0.5]) Sensor Left==1 Sesor Mid==1 Sensor Right==1 Plot ([x-0.5 x+0.5],[y- 0.5 y-0.5]) Plot ([x-0.5 x-0.5],[y- 0.5 y+0.5]) Plot ([x-0.5 x+0.5],[y- 0.5 y+0.5]) Sensor Left==1 Sesor Mid==1 Sensor Right==1 Plot ([x-0.5 x+0.5],[y- 0.5 y+0.5]) Plot ([x+0.5 x+0.5],[y- 0.5 y+0.5]) Plot ([x-0.5 x+0.5],[y- 0.5 y-0.5]) Sensor Left==1 Sesor Mid==1 Sensor Right==1 Plot ([x-0.5 x-0.5],[y- 0.5 y+0.5]) Plot ([x-0.5 x+0.5],[y+0.5 y+0.5]) Plot ([x+0.5 x+0.5],[y- 0.5 y+0.5]) Return R R Y Y N N Y Y N Y Y Y Y N Y N N N Y N Y Y N N N Y Y Y N N N N N
Hình 4.6. Giải thuật vẽ mê cung Thiết kế giao diện mê cung bằng phần mềm matlab GUI 4.5.
Giao diện điều khiển gồm có 3 phần chính: setting, send data, receive data, hiển thị mê cung 8x8.
Phần setting: dùng để lựa chọn cổng kết nối với máy tính, khi module ngoại vi được kết nối với máy tính thì giao diện sẽ nhận ra đó là cổng nào và hiển thị lên giao diện.
Tiếp theo là chọn tốc độ Baund Rate, tốc độ này cần đúng với tốc độ được set trong vi điều khiển cũng như modul UART.
Nút connect để kết nối giao diện matlab GUI với robot.
Phần receive data: khi có kết nối thì dữ liệu về mê cung được truyền lên giao diện GUI.
Giao diện mê cung: khi robot hoat động thì đường đi của robot sẽ được hiển thị lên phần này.
Hình 4.7. Giao diện hiển thị mê cung
Truyền nhận dữ liệu với máy tính:
Nhấn nút connect Robot được kết nối với máy tính qua giao diện Matlab GUI. Matlab sẽ load dữ liệu trong file mydata.txt để vẽ bản đồ.
Khi Robot bắt đầu di chuyển, nó sẽ được Reset về các thông số ban đầu (Huong=3, X=1, Y=1). Nếu di chuyển được một quãng đường đã định trước Robot lập tức truyền về thông tin của một ô trong mê cung mà nó thu thập được.
Matlab dựa trên thông tin truyền về để vẽ mê cung và lưu lại vào file mydata.txt.
Chƣơng 5: THỰC NGHIỆM ĐIỀU KHIỂN VÀ KẾT LUẬN Đánh giá
5.1.
5.1.1.Khả năng di chuyển của robot
Bằng việc sử dụng 2 động cơ dẫn động độc lập, tác giả luận văn đã xây dựng các hàm con để điều khiển robot di chuyển.
Hàm Turn Left() : giúp robot xoay góc 900 sang trái nhờ vào xử lí số encoder của từng động cơ và bộ điều khiển PID vị trí.
Hàm Turn Right(): giúp robot xoay góc 900 sang phải.
Hàm Run Straight(): giúp robot bám tường bên trái hoặc phải với khoảng cách xác định để xe di chuyển theo tường. Đọc encoder để đánh dấu từng ô robot đi qua.
Robot sẽ dừng khi gặp tường phía trước nhờ vào cảm biến siêu âm ở giữa hoặc so sánh số encoder trong một ô di chuyển.
5.1.2.Khả năng nhận dạng vật cản của bộ 3 cảm biến siêu âm:
Việc trang bị 3 cảm biến siêu âm ở ba hướng trái, phải, thẳng giúp cho mô hình có thể dễ dàng nhận biết được đặc tính của ô đang đứng trong mê cung như đường cụt, ngã ba, ngã tư, … Từ đó, song song với quá trình di chuyển qua các ô, robot cập nhật các mảng dữ liệu tường để xây dựng được bản đồ mê cung hoàn thiện.
5.1.3.Khả năng di chuyển trong mê cung theo thuật toán:
Bằng việc nhúng chương trình tự hành theo thuật toán Trémaux xuống vi điều khiển trung tâm, từ vị trí bắt đầu mặc định, robot có khả năng di chuyển trong mê cung theo các bước ưu tiên quy định trong thuật toán, nhận dạng các bức tường và xây dựng mảng dữ liệu mê cung.
Robot cũng có khả năng di chuyển theo đường ngắn nhất tới điểm đích khi đã được gởi lệnh từ máy tính theo thuật toán floodfill.
Thực nghiệm 5.2.
5.2.1.Mê cung thực nghiệm
Mê cung thực nghiệm là một mê cung bất kì được xây dựng theo kích thước ở chương 3 gồm 8x8 ô. Mô hình mê cung như hình sau:
Hình 5.1. Robot và 1 góc của mê cung 5.2.2.Kết quả thực nghiệm
Thực nghiệm với một mê cung chưa biết trước có các ngõ vào và ra IN, OUT như hình.
Tiến hành cho robot di chuyển trong mê cung theo các bước ưu tiên tại vị trí mặc định là IN. Robot chạy ba lần với các thứ tự ưu tiên đi thẳng, rẽ trái, rẽ phải như sau:
- Lần 1: Ưu tiên đi thẳng, nếu có tường ở phía trước thì rẽ trái, nếu có tường ở phía trước và bên trái thì rẽ phải.
- Lần 2: Ưu tiên đi thẳng, nếu có tường ở phía trước thì rẽ phải, nếu có tường ở phía trước và bên phải thì rẽ trái.
- Lần 3: Ưu tiên rẽ phải, nếu có tường bên phải thì đi thẳng, nếu có tường phía trước và bên phải thì rẽ trái.
Trong luận văn này tác giả không xét đến trường hợp Robot gặp cả tường bên trái, bên phải và phía trước.
Lần đầu tiên Robot bắt đầu dò mê cung, trong file mydata.txt của Matlab sẽ không có dữ liệu, giao diện Matlab GUI sẽ không có các đường đỏ biểu diễn bức tường.
Khi Robot bắt đầu di chuyển tại vị trí mặc định là IN, nó sẽ được set các thông số ban đầu (Huong=3, X=1, Y=1) theo quy định về hướng, tọa độ đã được nêu ở chương 4.
Nếu di chuyển được một quãng đường đúng bằng số xung encoder đã định trước Robot lập tức truyền về thông tin của một ô trong mê cung mà nó thu thập được bằng bộ 3 cảm biến siêu âm, từ đó người dùng có thể theo dõi trực tiếp các bước di chuyển của Robot.
Thông tin của một ô trong mê cung sẽ được cập nhật lần lượt vào file