8. Đề nghị: Được bảo vệ Bổ sung thêm để bảo vệ Không được bảo vệ
4.2.3 Giải thuật đọc cảm biến
a) Cảm biến siêu âm
Giải thuật đọc cảm biến siêu âm
Vì không đủ chân ngắt capture để đọc 4 cảm biến 1 lúc nên ta đƣa ra giải thuật đọc bằng chân IO: P0.4, P0.5, P0.6, P0.7. Và sử dụng chân P1.21 để phát xung kích.
Khi có tín hiệu phản xạ về thì chân IO tƣơng ứng sẽ lên mức 1. Khoảng cách tới vật cản dài hay ngắn thì thời gian chân IO lên mức 1 tƣơng ứng dài hay ngắn. Nhƣ vậy ta chỉ cần đo thời gian chân IO lên mức 1 thì sẽ suy ra đƣợc khoảng cách tới vật cản.
GVHD: HUỲNH THÁI HOÀNG Trang 79 Trong chƣơng trình này, sau khoảng thời gian 100us ta sẽ kiển tra chân IO 1 lần, nếu chân IO ở mức 1 thì ta tăng biến đếm sensor tƣơng ứng. Sau khoảng thời gian 30ms, ta sẽ lấy kết quả biến đếm Sensor để tính khoảng cách tới vật cản theo công thức.
Distance=(Sensor*100)/58
(Sensor*100) là độ rộng xung đọc về, số 58 là hệ số chia để tính ra khoảng cách, hệ số này đƣợc tìm thấy trong datasheet mà nhà sản xuất đƣa ra.
Hình 4.21 Giải thuật đọc cảm biến siêu âm
Khởi tạo
Reset các biến đếm
P0.4=1 P0.5=1 P0.6=1 P0.7=1
Sensor1 ++ Sensor2 ++ Sensor3 ++ Sensor4++
Timer=30ms Ngắt timer
100us
Tính khoảng cách
Sai Sai Sai Sai
Sai Đúng Đúng Đúng Đúng Đúng (Bắt cạnh lên xung đọc về) Đúng Sai Start End
GVHD: HUỲNH THÁI HOÀNG Trang 80 Khối “Tính khoảng cách” có nhiệm vụ tính khoảng cách đọc về từ các cảm biến:
Distance1=(Sensor1*100)/58
Distance2=(Sensor2*100)/58
Distance3=(Sensor3*100)/58
Distance4=(Sensor4*100)/58
Hình 4.22 Hình minh họa giản đồ xung đọc về từ cảm biến siêu âm
Vì cảm biến đƣợc đọc bằng cách đọc IO với chu kì 100us đọc IO 1 lần nên sẽ sai số khoảng (100/58)=1.72cm. Với sai số này thì vẫn đủ để điều khiển.
P1.21 10us
Độ rộng xung từ 100us tới 25ms P0.4
P0.5
P0.6
GVHD: HUỲNH THÁI HOÀNG Trang 81
b) La bàn điện tử
Hình 4.23 Giải thuật đọc la bàn điện tử
La bàn chỉ sử dụng trong trƣờng hợp ngƣời dùng muốn xe đi thẳng nhƣng do 2 bánh xe khởi động không đồng bộ nên có thể xe bị lệch khởi hƣớng mong muốn.
Khi ngƣời dùng muốn xe đi thẳng thì máy tính sẽ gửi xuống UART kí tự “f” (forward), lúc này ngắt UART kiểm tra nếu kí tự nhận đƣợc trong Buffer là “f” thì nó sẽ gọi hàm đọc góc la bàn về, và đây cũng chính là hƣớng mà ngƣời dùng muốn đi. Đồng thời trong ngắt này cũng sẽ lƣu vào biến trung gian Cmd kí tự trong Buffer.
Khi có ngắt timer1 100ms, nó sẽ kiểm tra nếu Cmd=‟f‟ thì sẽ đọc góc hiện tại của la bàn về và thực hiện PD giữa góc hiện tại và góc mong muốn. Sau đó sẽ xuất ra vận tốc xoay để điều chỉnh hƣớng xe. Khởi tạo Start Buffer=‟f ‟ Đọc góc mong muốn đi, và lƣu Cmd=‟f‟ Ngắt timer 100ms Cmd=‟f‟ End Đọc góc hiện tại Ngắt UART Đúng Đúng Sai Sai
GVHD: HUỲNH THÁI HOÀNG Trang 82
Chƣơng 5:
KẾT QUẢ VÀ HƢỚNG PHÁT TRIỂN
5.1 Kết quả
Xây dựng bộ driver điều khiển tốc độ động cơ BLDC công suất lớn.
Đã thiết kế và hoàn thiện mô hình phần cứng gồm 2 module điều khiển 2 động cơ.
Thiết lập bộ điều khiển PID trên 2 module động cơ với đáp ứng khá tốt, thời gian xác lập và thời gian lên hầu nhƣ chấp nhận đƣợc … ngay cả khi động cơ hoạt động có tải hay ở nhiều tốc độ khác nhau.
Xây dựng giải thuật điều khiển bằng joystick thông qua mạch master và đã hoạt động thành công.
Thiết lập truyền dữ liệu đồng bộ giữa 2 driver động cơ thông qua master dùng giao thức UART.
Xây dựng đƣợc giải thuật nhận dạng cử chỉ tay với tỉ lệ nhận dạng chính xác khá cao.
Đọc đƣợc tín hiệu từ cảm biến la bàn để điều khiển xe chạy dọc theo đƣờng thẳng.
Dùng giải thuật PD với tín hiệu đọc về từ la bàn để điều chỉnh hƣớng xe lăn.
Đọc đƣợc tín hiệu từ các cảm biến siêu âm để xe có thể tránh chƣớng ngại vật
5.1.1 Kết quả mạch điều khiển và mạch công suất
- Mạch điều khiển cho driver có khả năng cách ly và không bị ảnh hƣởng bởi những tín hiệu không mong muốn từ mạch công suất.
- Mạch công suất đƣợc thiết kế dùng 2 FET trên mỗi nửa cầu và có khả năng chịu đƣợc dòng lên đến 150A (theo lý thuyết), áp giới hạn là 75V.
GVHD: HUỲNH THÁI HOÀNG Trang 83 Hình 5.1 Mạch công suất
GVHD: HUỲNH THÁI HOÀNG Trang 84
GVHD: HUỲNH THÁI HOÀNG Trang 85