Thiết kế mạch robot dò line BKIT MCR
1 Các hàm 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 phải Giá trị truyền vào left right từ -255 đến 255 void handle(int angle); Điều chỉnh góc động servo so với phương vng góc với thân xe Đơn vị đối số angle độ ( ) Xem hình 3.1 unsigned char sensor_inp(unsigned char MASK); Hàm trả giá trị sensor dò trước đầu xe sau AND với MASK Ví dụ: Cấu trúc chương trình Chương trình chia làm trạng thái chính: • Bước chuyển (1: I -> II) Khi thấy nửa line bên trái: • Bước chuyển (2: II -> I) Khi qua đoạn đường chuyển trái • Bước chuyển (3: I -> III) Khi thấy nửa line bên phải • Bước chuyển (4: III -> I) Khi chạy qua đoạn chuyển phải • Bước chuyển (5: I -> IV), (7: II -> IV), (8: III -> IV) Ngay thấy nguyên line • Bước chuyển (6: IV -> I) Khi chạy xong đoạn đường cua 90 độ ***Chú ý: * Ưu điểm sơ đồ giải thuật giải lỗi nhầm lẫn đoạn đường cua vuông thành đoạn đường chuyển Thường khơng phải lúc mạch dò song song với vạch trắng ngang, có bên mạch dò bắt vạch trắng trước ( hình 5.7 ) sinh lỗi nhầm lẫn đoạn đường cua vuông thành chuyễn làn, việc thường xuyên xảy * Khi mạch dò ngang qua vạch trắng mà mạch dò nằm lệch qua phía vạch trắng bị nhầm lẫn đoạn đường chuyển thành đoạn cua vng ( hình 5.8 ), thường đường đua thật chuẩn qui định lỗi xảy Lỗi gặp sau đường cong hai vạch trắng chuyển xe chưa kịp chỉnh mạch dò vào đường đua Khi xảy lỗi khơng đáng ngại với kỹ thuật qua đoạn cua vng xe dễ dàng qua đoạn đường chuyển vận tốc xe chậm so với không nhầm lẫn Giải thuật xử lý qua đoạn đường thẳng cong - Các trạng thái sensor gặp thể tương đối độ lệch hướng xe so với phương đường đua: - Cách tính tỉ lệ vận tốc hai bánh sau theo góc bẻ lái hai bánh trước: Từ hình vẽ ta có: Từ (1), (2), (3) suy tỉ lệ vận tốc hai bánh theo góc bẻ lái: Ví dụ: T = 0,14 m W = 0,175 m Ta có bảng sau: - Sơ đồ giải thuật trạng thái I ( xem hình 3-11): - Ý tưởng qua đoạn đường thẳng cong: Ta dựa vào trạng thái sensor để biết độ lệch hướng chạy xe so với phương đường đua, ứng với trạng thái sensor ta chọn góc bẻ lái hợp lý ( cách dùng hàm handle (int) ) cho xe có xu hướng di chuyển phía đường đua, với độ lệch hướng lớn ta chọn góc bẻ lái lớn Ưu điểm cách chạy là: xe chạy qua đường cong với bán kính cố định nhanh giống chạy đường thẳng xe vào đường cong Thực tế thi MCR có hai loại đường cong với hai bán kính khác Nên ta cần chọn hai góc bẻ lái hợp lý ứng với hai độ lệch hướng khác để qua loại đường cong cho hai độ lêch hướng chọn hai độ lệch hướng trung bình – cụ thể độ lệch hướng ứng với biến old_state = +/-3 old_state = +/-4 (xem hình 5.9 trên) Tại khúc chuyển giao đường thẳng sang đường cong đường cong sang đường cong hướng khác ta tắt động phát động để toàn nguồn cung cấp cho rc servo bẻ lái ( thực tế 20 – 40 ms ) để xe chạy vận tốc ổn định cao bình thường Và phát xe lệch khỏi phương đường đua xa mạch dò bắt line line biên ta cho xe hãm tốc mạnh để tránh trường hợp xe văng khỏi đường đua - Ý tưởng thực 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 hai line biên - Biến brake_flag, hàm brake(int time), hàm brake_timer (int time, int_ speed) dùng để thực ý tưởng cho xe hãm tốc 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) hàm hãm tốc cách cho vận tốc hai bánh (speed (0,0) ) mà điều khiển bẻ lái servo cho xe hướng theo line tương tự hàm runforwardline ( ) - Hàm brake_timer (int time, int_ speed ) tương tự hàm brake ( int time) ta chỉnh vận tốc hai bánh thông số _speed truyền vào hàm Hàm dùng để hãm tốc mạnh Sơ đồ trạng thái qua đoạn đường cua vuông - Thứ tự kiện qua đoạn cua vng: - Sơ đồ trạng thái: Hình 3-16 Gắn kết nối PIN vào thân xe Hình 3-17 Bắt ốc kết nối mạch vào thân xe Bước 3: Bắt mạch nguồn vào thân xe Hình 3-18 Bắt mạch nguồn vào thân xe Bước 4: Bắt mạch dò đường Hình 3-19 Bắt mạch dò đường Hình 3-20 Bắt mạch nhận cổng mạch dò đường Bước 5: Bắt mạch vi điều khiển vào trục ốc thân xe Hình 3-21 Chuẩn bị lắp mạch vi điều khiển vào thân xe Hình 3-22 Bắt mạch vi điều khiển kết nối dây vào thân xe Bước 6: Bắt mạch công suất vào thân xe Hình 3-23 Bắt mạch cơng suất vào thân xe Hình 3-24 Bắt ốc kết nối bus mạch cơng suất Hình 3-25 Gắn PIN vào thân xe Hình 3-26 Kết nối bus, hồn thành việc lắp ráp xe BKIT MCR ... xe chạy Với DipSW-4 ta chọn 16 mode tứ mode đến mode 15 (0b0000 -> 0b 111 1) Khi lập trình chân PC3:0 ta cài đặt chế độ input kéo lên Khi công-tắc hở (OFF) ta nhận mức 1, cơng-tắc đóng (ON) ta nhận... Từ hình vẽ ta có: Từ (1) , (2), (3) suy tỉ lệ vận tốc hai bánh theo góc bẻ lái: Ví dụ: T = 0 ,14 m W = 0 ,17 5 m Ta có bảng sau: - Sơ đồ giải thuật trạng thái I ( xem hình 3 -11 ): - Ý tưởng qua đoạn... key_input, biến key0, key1, key2 để lưu giá trị nút nhấn trạng thái liên tiếp nhau: Code: void update_key(){ key2 = key1; key1 = key0; key0 = PINC; if ((key0 == key1) && (key1 == key2)){ key_input