Đang tải... (xem toàn văn)
Untitled BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ[.]
BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH - ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ- TRUYỀN THƠNG ĐỀ TÀI: Thiết kế thi cơng xe lăn nhà tự định vị đến đích dựa vào đồ 2D GVHD: ThS Ngô Bá Việt SVTH: Lê Trung Nghĩa MSSV: 15141220 SVTH: Nguyễn Lữ Phụng Đình MSSV: 15141130 Tp Hồ Chí Minh - 12/2020 BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH o0o ĐỒ ÁN TỐT NGHIỆP NGÀNH KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: Thiết kế thi công xe lăn nhà tự định vị đến đích dựa vào đồ 2D GVHD: ThS Ngô Bá Việt SVTH: Lê Trung Nghĩa MSSV: 15141220 SVTH: Nguyễn Lữ Phụng Đình MSSV: 15141130 Tp Hồ Chí Minh - 12/2020 i LỜI CAM ĐOAN Chúng em cam đoan đồ án tốt nghiệp cơng trình tự thực dựa vào tài liệu trước nghiên cứu hướng dẫn Thạc sĩ Ngô Bá Việt Các kết đề tài “Thiết kế thi công xe lăn nhà tự định vị đến đích dựa vào đồ 2D” trung thực khơng chép từ cơng trình khác Người thực đề tài Lê Trung Nghĩa – Nguyễn Lữ Phụng Đình vi LỜI CẢM ƠN o0o -Chúng em xin chân thành cảm ơn quý Thầy Cô Trường Đại học Sư phạm Kỹ thuật Tp.HCM nói chung, thầy Bộ mơn Điện tử Cơng Nghiệp – Y Sinh nói riêng tận tình giảng dạy, truyền đạt cho chúng em kiến thức quý báu, đam mê khám phá học hỏi chúng em tạo điều kiện giúp đỡ suốt trình học tập năm học vừa qua, giúp chúng em có sở lý thuyết vững vàng, có lý tưởng tri thức để sau ứng dụng vào công việc, sống Chúng em xin gửi lời cảm ơn chân thành đến Th.S Ngô Bá Việt - Trường Đại học Sư phạm Kỹ thuật Tp.HCM tận tâm,tân lực hướng dẫn bảo suốt thời gian làm luận án tốt nghiệp Thầy tạo nhiều điều kiện cho lời khuyên quý báu giúp chúng em hoàn thành tốt khóa luận Tuy q trình thực có nhiều khó khăn, trăn trở, nhờ hướng dẫn động viên, chúng em vượt qua thành công Cuối cùng, chúng em xin chân thành cảm ơn gia đình bạn bè, ln tạo điều kiện, quan tâm, ủng hộ, giúp đỡ, động viên chúng em suốt trình học tập hoàn thành đồ án tốt nghiệp.Chúng em khắc sâu công ơn đường nghiệp Xin chân thành cảm ơn! Người thực đề tài Lê Trung Nghĩa – Nguyễn Lữ Phụng Đình vii MỤC LỤC TRANG BÌA i NHIỆM VỤ ĐỒ ÁN ii LỊCH TRÌNH THỰC HIỆN iv LỜI CAM ĐOAN vi LỜI CẢM ƠN vii MỤC LỤC viii LIỆT KÊ HÌNH ẢNH xi LIỆT KÊ BẢNG VẼ xiv TÓM TẮT xv CHƯƠNG TỔNG QUAN 1.1 ĐẶT VẤN ĐỀ 1.2 MỤC TIÊU ĐỀ TÀI 1.3 NỘI DUNG NGHIÊN CỨU 1.4 GIỚI HẠN ĐỀ TÀI 1.5 BỐ CỤC CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 PHƯƠNG PHÁP ĐIỀU KHIỂN XE LĂN ĐẾN ĐÍCH 2.2 PHƯƠNG PHÁP VẼ QUỸ ĐẠO ĐƯỜNG ĐI CỦA XE LĂN 2.3 GIỚI THIỆU PHẦN CỨNG 2.3.1 Kit Arduino DUE 2.3.2 Encoder 2.3.3 Module Dc-Dc Xl4005 2.3.4 Module Điều Khiển Động Cơ 10 2.3.5 Động Cơ Dc 11 2.3.6 Lcd Nextion 13 2.3.7 Cảm Biến Siêu Âm Hc-Sr05 15 CHƯƠNG TÍNH TỐN VÀ THIẾT KẾ 17 3.1 GIỚI THIỆU 17 3.2 TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG 17 3.2.1 Thiết kế sơ đồ khối hệ thống 17 viii 3.2.2 Sơ đồ nguyên lý nối dây module 18 3.2.3 Thiết kế khối hệ thống 18 3.2.4 Module điều khiển động BTS7960 27 3.2.5 Khối nguồn 28 3.2.6 Tính tốn tránh cật cản dùng cảm biến siêu âm 28 CHƯƠNG THI CÔNG HỆ THỐNG 31 4.1 GIỚI THIỆU 31 4.2 THI CÔNG HỆ THỐNG 31 4.2.1 Thi công bo mạch 31 4.2.2 Láp ráp kiểm tra 33 4.2.3 Kết thi công bo mạch 34 4.2.4 Kết thi công hộp đựng module cảm biến siêu âm SRF05 34 4.2.5 Kết thi cơng Module hình cảm ứng LCD 35 4.2.6 Kết thi cơng mơ hình 37 4.3 THIẾT KẾ GIAO DIỆN CHO NGƯỜI ĐIỀU KHIỂN 39 4.4 LẬP TRÌNH HỆ THỐNG ARDUINO ĐIỀU KHIỂN 40 4.4.1 Cấu hình phiên làm việc cho Arduino DUE 40 4.4.2 Lập trình kit Arduino DUE 41 4.4.2.1 Sơ đồ khối tổng quát Arduino 41 4.5 VIẾT TÀI LIỆU HƯỚNG DẪN SỬ DỤNG, THAO TÁC 47 4.5.1 Viết tài liệu hướng dẫn sử dụng 47 4.5.2 Quy trình thao tác giao diện 48 CHƯƠNG KẾT QUẢ_NHẬN XÉT_ĐÁNH GIÁ 49 5.1 GIỚI THIỆU 49 5.2 KẾT QUẢ THỰC NGHIỆM ĐIỀU KHIỂN XE LĂN TRONG TRƯỜNG HỢP KHÔNG VẬT CẢN 49 5.3 KẾT QUẢ THỰC NGHIỆM ĐIỀU KHIỂN XE LĂN TRONG TRƯỜNG HỢP CÓ VẬT CẢN 54 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 60 6.1 KẾT LUẬN 60 6.2 HƯỚNG PHÁT TRIỂN 60 TÀI LIỆU THAM KHẢO PHỤ LỤC ix CODE ARDUINO x LIỆT KÊ HÌNH VẼ Hình Trang Hình 2.1: Phương pháp đường cột mốc chủ động Hình 2.2 : Phương pháp sử dụng đồ Hình 2.3 : Board Arduino DUE Hình 2.4 : Sơ đồ chức chân Arduino DUE Hình 2.5 : Giao tiếp hệ thống dùng chuẩn UART Hình 2.6 : Nguyên lý hoạt động chuẩn UART Hình 2.7 : Cấu tạo encoder Hình 2.8 : Nguyên lý hoạt động encoder Hình 2.9 : Module DC-DC XL4005 5A 10 Hình 2.10 : Module BTS7960 43A 10 Hình 2.11 : Sơ đồ chân Module Điều Khiển Động Cơ BTS7960 43A 11 Hình 2.12 : Cấu tạo động DC với phần than lộ phần rotor dây đồng 13 Hình 2.13 : Nextion 3.2 inch NX4024T032 15 Hình 2.14 : Cảm biến siêu âm HC-SR05 15 Hình 3.1 Sơ đồ khối hệ thống 17 Hình 3.2: Sơ đồ nguyên lý 18 Hình 3.3 : Thử nghiệm kiểm tra quỹ đạo xe 19 Hình 3.4 : Thử nghiệm kiểm tra quỹ đạo xe sau bơm căng hai bánh 20 Hình 3.5 : Thử nghiệm kiểm tra cho xe chạy theo hướng ngược lại 20 Hình 3.6 : Phương pháp chỉnh cho xe 21 Hình 3.7 : Căn chỉnh thông số xe 22 Hình 3.8 : Kết chạy sau nạp lại thông số cho bánh xe 22 Hình 3.9 : Module LCD cảm ứng Nextion 23 Hình 3.10 : Sơ đồ nối dây vi điều khiển Broad Arduino DUE 24 Hình 3.11 : Sơ đồ nối dây mạch nạp Arduino DUE 26 Hình 3.12 : Module LM2596 27 Hình 3.13 : Bình Ắc quy dùng cho xe 28 Hình 3.14 : Hình hoạt động siêu âm 29 Hình 3.15 : Hình sơ đồ thời gian HC-SR05 chế độ 29 Hình 3.16 : Hình sơ đồ thời gian HC-SR05 chế độ 30 xi Hình 4.1: PCB nối dây module kích thước 32 Hình 4.2 : Mạch in PCB 32 Hình 4.3 : Bảng mạch in hoàn chỉnh 33 Hình 4.4 : Hình ảnh thực tế sau thi cơng 33 Hình 4.5 : Kết thi công board mạch 34 Hình 4.6 : Hộp đựng cảm biến siêu âm 34 Hình 4.7 : Hộp đựng gắn vào khung xe 35 Hình 4.8 : Mặt hộp đựng LCD 35 Hình 4.9 : Mặt ngồi hộp đựng LCD 36 Hình 4.10 : Giá quay đỡ hình LCD 36 Hình 4.11 : Gắn hộp đựng mạch điện vào thân bình Ắc quy 37 Hình 4.12 : Gắn hộp đựng cảm biến siêu âm 37 Hình 4.13 : Gắn hộp đựng hình LCD 37 Hình 4.14 : Gắn encoder vào bánh xe 38 Hình 4.15 : Kết thi cơng mơ hình xe lăn 38 Hình 4.16 : Tồn cảnh mơ hình xe lăn 39 Hình 4.17 : Giao diện điều khiển Nextion 39 Hình 4.18 : Giao diện nạp vào hình Nextion 40 Hình 4.19 : Tải board Arduino DUE 40 Hình 4.20 : Chọn board Arduino DUE ngõ nạp 40 Hình 4.21 : Chọn cổng Arduino 41 Hình 4.22 : Sơ đồ khối chương trình truyền liệu ngoại vi sang Arduino DUE 41 Hình 4.23 : Lưu đồ điều khiển xe tự tìm đường 42 Hình 4.24 : Lưu đồ xe tự di chuyển đến tọa độ nằm góc phần tư thứ 43 Hình 4.25 : Lưu đồ xe tự di chuyển đến tọa độ nằm góc phần tư thứ 44 Hình 4.26 : Lưu đồ xe tự di chuyển đến tọa độ nằm góc phần tư thứ 45 Hình 4.27 : Lưu đồ xe tự di chuyển đến tọa độ nằm góc phần tư thứ 46 Hình 4.28 : Lưu đồ xe tự tránh vật cản 47 Hình 5.1 : Tiến hành thử nghiệm 50 Hình 5.2 : Mơ hình xe lăn tự động vận hành 50 Hình 5.3 : Mơ hình xe lăn tự động tiến đích 51 Hình 5.4 : Mơ hình xe lăn tự động tới đích 51 xii Hình 5.5 : Dữ liệu quỹ đạo kết thu 52 Hình 5.6 : Dữ liệu quỹ đạo kết thực nghiệm số 52 Hình 5.7 : Dữ liệu quỹ đạo kết thực nghiệm 53 Hình 5.8 : Bố trí vật cản cho thực nghiệm 55 Hình 5.9 : Mơ hình xe lăn tự động phát vật cản 55 Hình 5.10 : Mơ hình xe lăn tự động tránh vật cản 56 Hình 5.11 : Mơ hình xe lăn tự động tiếp tục tránh vật cản 56 Hình 5.12 : Mơ hình xe lăn tự động tránh vật cản 57 Hình 5.13 : Mơ hình xe lăn tự động đến đích 57 Hình 5.14 : Dữ liệu quỹ đạo kết thực nghiệm 58 xiii CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Bảng 5.1 : Đánh giá kết thực nghiệm Thực Tọa độ đích Tọa độ thực Sai số x Sai số Thời gian xe nghiệm (m) (m) (%) y(%) lăn tự động vận hành (s) X = 2, Y = x = 2.03, y = 5.02 1.5 0.4 39 X = 2, Y = x = 2.02, y = 2.01 0.5 19 X = 2, Y = 10 x = 2.01, y = 10.08 0.5 0.8 46 Từ bảng 5.1 cho thấy vận hành khoảng cách gần 100m mơ hình xe lăn tự động xe có độ xác 98%, với điều kiện mặt sàn có độ nhám tốt, khơng bị ướt hay trơn trượt Sai số tích lũy ảnh hưởng q trình mơ hình xe lăn tự động di chuyển khơng trơn tru mơ hình xe lăn tự động phải liên tục tự điều chỉnh gây tượng q đà làm cho mơ hình xe lăn tự động bị lết bánh lúc thẳng, di chuyển nhanh, dừng lại đột ngột ảnh hưởng đến xác mơ hình xe lăn tự động Hơn thời gian xử lý tính tốn thời gian thực khơng thực trung khớp nên có độ trễn dịnh , mơ hình xe lăn tự động ln bị thừa khoảng nhỏ sau cán đích 5.4 KẾT QUẢ THỰC NGHIỆM ĐIỀU KHIỂN XE LĂN TỰ ĐỘNG TRONG TRƯỜNG HỢP CÓ VẬT CẢN Tương tự thực nghiệm điều khiển xe lăn tự động trường hợp không vật cản đường mơ hình xe lăn tự động bố trí thêm vật cản Các mốc điểm đo đạc khoảng cách từ trước đường thẳng nối mốc điểm lại với đường lí tưởng, ngồi trường hợp cịn có kiện né tránh vật cản nên đường lí tưởng có thêm mốc điểm lí tưởng nơi mơ hình xe lăn phát vật cản, khoảng cách phát vật cản thiết lập cho mơ hình xe lăn tự động 50cm Trên mơ hình xe lăn tự động có mang theo máy vi tính để kết nối serial lưu liệu mơ hình xe lăn tự động để so sánh kết thực tế với lý tưởng Cài đặt vận tốc cho mơ hình xe lăn tự động chạy chậm 30cm/s BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 54 CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Hình 5.8 : Bố trí vật cản cho thực nghiệm Hình 5.8 mơ tả vật cản hai ván ép dựng đứng tựa vào ghế, đặt hai vật mốc chai nước rửa tay hộp nhựa tức cách chai nước rửa tay khoảng 1m Hình 5.9 : Mơ hình xe lăn tự động phát vật cản Hình 5.9 mơ tả mơ hình xe lăn tự động phát vật cản nằm khoảng cách báo động mơ hình xe lăn tự động ngưng lại, chuẩn bị rẽ trái (quy ước mặc định) BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 55 CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Hình 5.10 : Mơ hình xe lăn tự động tránh vật cản Hình 5.10 mơ tả mơ hình xe lăn tự động rẽ trái để tránh (mặc định rẽ trái trước), lại phát vật cản khác (máy y tế) nằm vùng báo động, mơ hình xe lăn tự động ngưng lại Hình 5.11 : Mơ hình xe lăn tự động tiếp tục tránh vật cản Hình 5.11 mơ tả mơ hình xe lăn tự động quay phải sau hai lần phát vật cản phía tay trái Hiện xe xoay sang phải cảm biến siêu âm chưa phát vật cản BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 56 CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Hình 5.12 : Mơ hình xe lăn tự động tránh vật cản Hình 5.12 mơ tả mơ hình xe lăn tự động sau lúc quay khơng phát vật cản vùng báo động tiếp tục di chuyển thẳng tiếp tục tìm đường di chuyển đích Hình 5.13 : Mơ hình xe lăn tự động đến đích Hình 5.13 mơ tả mơ hình xe lăn tự động sau đến đích dừng lại, q trình di chuyển, mơ hình xe lăn tự động liên tục tự điều chỉnh để di chuyển không bị lệch đường thẳng BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 57 CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Hình 5.14 : Dữ liệu quỹ đạo kết thực nghiệm Hình 5.14 mô tả quỹ đạo đường thực tế (màu xanh) lí tưởng (màu đỏ) mơ hình xe lăn tự động Đoạn từ A đến B , mô hình xe lăn tự động di chuyển bám theo đoạn thẳng lí tưởng sai số nhỏ 0.15cm , đoạn B đến C mơ hình xe lăn tự động di chuyển đúng, sai số nhỏ 0.1cm , đoạn từ C đến D mơ hình xe lăn tự động bị lệch dần khoảng 4cm phía bên trái sau lại ổn định dần phía điểm D Đến điểm D , mơ hình xe lăn tự động chưa dừng lại tiếp tục di chuyển sai lệch theo trục x thêm 3.5cm dừng lại, D mô hình xe lăn tự động quay trái di chuyển điểm E có tọa độ X = 2m, Y = 5m , cán mốc điểm E , mô hình xe lăn tự động chưa dừng lại tiếp tục di chuyển sai lệch theo trục Y thêm 2cm Tọa độ thực mơ hình xe lăn tự động lúc x = 2.015m, y = 5.02m, thời gian mơ hình xe lăn tự động di chuyển từ điểm A đến điểm E khoảng 44 giây Bảng 5.2 : Đánh giá kết thực nghiệm trường hợp có vật cản Đoạn Độ dài lí tưởng Độ dài thực tế (m) (m) AB 0.5 0.5016 0.32 BC 0.25 0.2501 0.04 CD 1.5 1.536 2.4 DE 4.75 4.775 1.58 ABCDE 7.0627 0.89 BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH Sai số (%) 58 CHƯƠNG KẾT QUẢ NHẬN XÉT ĐÁNH GIÁ Từ bảng 5.2, kết chi tiết đoạn đoạn cho sai số thấp , độ xác lên tới gần 99% cho khoảng cách di chuyển vòng 7m Với điều kiện thường xuyên phải kiểm tra lại độ căng hai bánh xe thực lại bước cân chỉnh thông số qui đổi bánh xe Encoder cho Arduino DUE BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH 59 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Chương KẾT LUẬN - HƯỚNG PHÁT TRIỂN 6.1 KẾT LUẬN Mơ hình xe tự động tìm đường đích né tránh chướng ngại vật mà mơ hình phát đường Mơ hình định vị vị trí với sai số nhỏ Mơ hình có chế độ Auto Manual, sử dụng giao tiếp LCD cảm ứng Nextion Arduino DUE để điều khiển thiết bị ngoại vi Tuy nhiên bên cạnh kết đạt được, cịn có yếu điểm dễ bị ảnh hưởng môi trường vận hành địa hình gồ ghề, mặt sàn trơn, thuật tốn xử lý chưa tối ưu hồn tồn nên gây tượng xe chạy giật, rung lắc không trơn tru Mơ hình mang tính nghiên cứu, chưa hồn chỉnh 6.2 HƯỚNG PHÁT TRIỂN Để có đề tài mang đủ tính chất ứng dụng vào thực tiễn cách cao hơn, nên cải tiến thêm hướng: - Thêm camera lidar để tối ưu nhận biết vị trị vật cản nhà, hạn chế sử dụng nhiều cảm biến - Bánh xe nên sử dụng loại đặc ruột, không bơm - Khung gầm xe nên gắn thêm phuộc nhún để hạn chế rung lắc khơng cần thiết - Tìm kiếm thuật toán xử lý di chuyển tối ưu sử dụng điều khiển PID, Fuzzy để mô hình vận hành trơn tru, giảm thiểu rung lắc gây sai số - Có thể nâng cấp thêm điều khiển sóng điện não - Tìm kiếm giải thuật tăng độ xác sai sơ cộng dồn encoder - Sử dụng thiết bị có cấu hình cao để đáp ứng khả xử lý tốt - Thêm thuật tốn tìm đường AI, mạng Neuron để giúp hệ thống phát triển tối ưu BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP- Y SINH 60 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO Sách tham khảo [1] P.H:Việt Nam có triệu người khuyết tật http://cand.com.vn/Xa-hoi/dam- bao-quyen-va-loi-ich-cua-nguoi-khuyet-tat-375574/ [2] Đình Nam: Sinh viên Việt sáng chế xe lăn thông minh, robot dùng smartphone https://vnexpress.net/sinh-vien-viet-sang-che-xe-lan-thong-minh-robot-dungsmartphone-3346928.html [3] Khang Huy: Xe lăn hoàn toàn tự động https://thanhnien.vn/doi-song/khoa-hoc/xelan-hoan-toan-tu-dong-94105.html [4] Khánh Linh: https://automation.net.vn/Robot-Robotics/Mot-so-ky-thuat-dinh-vi- cho-Robot-di-dong.html [5] Robot mobile gì? https://baoanjsc.com.vn/tin-hang/mobile-robots-la- gi_2_1_31453_vn.aspx [6] Những phương pháp dẫn đường phổ biến: http://giamsatgps.vn/t/nhung-phuongphap-dan-duong-pho [7] Hans-Petter Halvorsen, Introduction to ARDUINO, 2008 [8] Odometry error correction by sensor fusion for autonomous mobile robot navigation, IEEE, author: C.TarinSauer, H.Brugger,E.P.Hofer,B.Tibken: https://ieeexplore.ieee.org/document/929484/authors#authors BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP PHỤ LỤC PHỤ LỤC ĐƯA VÀO DATASHEET TOÁN HỌC CƠ BẢN LIÊN QUAN ĐẾN ĐỀ TÀI ĐOẠN CODE CHƯƠNG TRÌNH #include #include #include //#include #include Encoder leftEncoder (22, 23); Encoder rightEncoder (25, 24); volatile float oldLeft, newLeft = 0; volatile float oldRight, newRight = 0; boolean chedo = true ; unsigned long previousMillis = 0; const long interval = 100; uint16_t len1; uint16_t number1; uint16_t len; uint16_t number; double leftVelocity = 0, rightVelocity = , x = , y = , x_old = , y_old = 0; float theta = 0, thetaold = 0; int LEFT_FORWARD = 13; int LEFT_REVERSE = 12; int RIGHT_FORWARD = 9; int RIGHT_REVERSE = 8; int EN_LEFT = 11; //left int EN_RIGHT = 10; // en r int fadeValue = 20 ; int left_res = 2048; int right_res = 1000; double d_l = 0.457135; double d_r = 0.457199; double L = 0.535; double pi = 3.1415926535897932; double cv_l = (0.457135 * pi) / 8192; double cv_r = (0.457199 * pi) / 4000; double X_Des = 0; double Y_Des = ; void timer_3_interrupt_for_left_motor () BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xiv PHỤ LỤC { newLeft = leftEncoder.read (); leftVelocity = newLeft - oldLeft ; return; } void timer_4_interrupt_for_right_motor () { newRight = rightEncoder.read (); rightVelocity = newRight - oldRight ; } void setup() { pinMode(22 , INPUT_PULLUP); pinMode(23 , INPUT_PULLUP); pinMode(24 , INPUT_PULLUP); pinMode(25 , INPUT_PULLUP); double cv_l = d_l * pi; double cv_r = d_r * pi; Timer3.attachInterrupt (timer_3_interrupt_for_left_motor); Timer3.start (10000); //100ms Timer4.attachInterrupt (timer_4_interrupt_for_right_motor); Timer4.start (10000); //100ms pinMode(LEFT_FORWARD, OUTPUT); pinMode(LEFT_REVERSE, OUTPUT); pinMode(RIGHT_FORWARD, OUTPUT); pinMode(RIGHT_REVERSE, OUTPUT); pinMode(EN_LEFT, OUTPUT); pinMode(EN_RIGHT, OUTPUT); Serial.begin(9600); HC06.begin(9600); } void loop() { unsigned long currentMillis = millis(); else if (number1 == 57) { Y_Des = ; X_Des = ; } else if (number == 56) { xuly(); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xv PHỤ LỤC } theta = (rightVelocity * cv_r - leftVelocity * cv_l) / L + thetaold; x = cos(theta) * (leftVelocity * cv_l + rightVelocity * cv_r) / + x_old ; y = sin(theta) * (leftVelocity * cv_l + rightVelocity * cv_r) / + y_old ; oldLeft = newLeft; oldRight = newRight; thetaold = theta; x_old = x; y_old = y ; if (currentMillis - previousMillis >= interval) { previousMillis = currentMillis; Serial.print(leftVelocity ); Serial.print(" ; "); Serial.print(rightVelocity ); Serial.print(" ; "); Serial.print(X_Des); Serial.print(" ; "); Serial.print(Y_Des); Serial.print(" ; "); Serial.print(x); Serial.print(" ; "); Serial.print(y); Serial.print(" ; "); Serial.print(theta * 180 / pi); Serial.print(" ; "); Serial.print(bluetoothByte); Serial.println(); } oldLeft = newLeft; oldRight = newRight; delay(100); } void dunglai() { digitalWrite(EN_LEFT, LOW); digitalWrite(EN_RIGHT, LOW); delay(10); analogWrite(LEFT_FORWARD, 0); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 0); analogWrite(RIGHT_REVERSE, 0); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xvi PHỤ LỤC } void dithang() { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(10); analogWrite(LEFT_FORWARD, 35); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 32); analogWrite(RIGHT_REVERSE, 0); } void lui() { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(10); analogWrite(LEFT_FORWARD, 0); analogWrite(LEFT_REVERSE, 30); analogWrite(RIGHT_FORWARD, 0); analogWrite(RIGHT_REVERSE, 30); } void quaytrai() { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(10); analogWrite(LEFT_FORWARD, 0); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 20); analogWrite(RIGHT_REVERSE, 0); } void quayphai() { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(10); analogWrite(LEFT_FORWARD, 20); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 0); analogWrite(RIGHT_REVERSE, 0); } void xuly() { BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xvii PHỤ LỤC if ((x >= X_Des) && ( y >= Y_Des )) { // dunglai(); digitalWrite(EN_LEFT, LOW); digitalWrite(EN_RIGHT, LOW); delay(20); analogWrite(LEFT_FORWARD, 0); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 0); analogWrite(RIGHT_REVERSE, 0); } else { if ( x < X_Des ) { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(20); analogWrite(LEFT_FORWARD, 35); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 35); analogWrite(RIGHT_REVERSE, 0); } else { if ( y < Y_Des ) { if ( Y_Des > 0) { if ((theta * 180 / pi) < 91) { quaytrai(); digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(20); analogWrite(LEFT_FORWARD, 0); analogWrite(LEFT_REVERSE, 20); analogWrite(RIGHT_FORWARD, 20); analogWrite(RIGHT_REVERSE, 0); BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xviii PHỤ LỤC } else // ((theta * 180 / pi) > 91) { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(20); analogWrite(LEFT_FORWARD, 20); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 0); analogWrite(RIGHT_REVERSE, 0); } if (((theta * 180 / pi) > 89) && ((theta * 180 / pi) < 91)) { digitalWrite(EN_LEFT, HIGH); digitalWrite(EN_RIGHT, HIGH); delay(20); analogWrite(LEFT_FORWARD, 35); analogWrite(LEFT_REVERSE, 0); analogWrite(RIGHT_FORWARD, 35); analogWrite(RIGHT_REVERSE, 0); } } } } } } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP xix S K L 0