(Đồ án hcmute) thiết kế và thi công xe lăn trong nhà tự định vị và đến đích dựa vào bản đồ 2d

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Ử ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ - TRUYỀN THÔNG THIẾT KẾ VÀ THI CÔNG XE LĂN TRONG NHÀ TỰ ĐỊNH VỊ VÀ ĐẾN ĐÍCH DỰA VÀO BẢN ĐỒ 2D GVHD: ThS NGƠ BÁ VIỆT SVTH: LÊ TRUNG NGHĨA MSSV: 15141220 SVTH: NGUYỄN LỮ PHỤNG ĐÌNH MSSV: 15141130 SKL 0 Tp Hồ Chí Minh, tháng 12/2020 an 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 an 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 an i NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Lê Trung Nghĩa Nguyễn Lữ Phụng Đình Kỹ thuật Điện Tử Truyền Thơng Đại học quy 2015 MSSV: 15141220 MSSV: 15141130 Chuyên ngành: Mã ngành: 01 Hệ đào tạo: Mã hệ: 01 Khóa: Lớp: 15141DT1B 15141DT2A I TÊN ĐỀ TÀI: THIẾT KẾ VÀ THI CÔNG XE LĂN TRONG NHÀ TỰ ĐỊNH VỊ VÀ ĐẾN ĐÍCH DỰA VÀO BẢN ĐỒ 2D II NHIỆM VỤ Các số liệu ban đầu: - Module Bluetooth HC06, LCD cảm ứng NEXTION - Arduino Due R3, Module Động Cơ BTS7960, Battery 12V,Moudle hạ áp 5A DC-DC XL4005 Nội dung thực hiện: Nhóm chúng em thực nội dung sau: - Nội dung 1: Kết nối Arduino với mạch cầu H điều khiển động - Nội dung 2: Kết nối Encoder Arduino - Nội dung 3: Đọc tính tốn vị trí xe lăn dựa vào encoder - Nội dung 4: Tính tốn đường xe lăn dựa vào đồ 2D - Nội dung 5: Thiết kế mơ hình hệ thống - Nội dung 6: Thi cơng phần cứng, chạy thử nghiệm hiệu chỉnh hệ thống III NGÀY GIAO NHIỆM VỤ: 05/10/2020 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 20/01/2021 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: ThS Ngô Bá Việt CÁN BỘ HƯỚNG DẪN BM ĐIỆN TỬ CÔNG NGHIỆP – Y SINH ii an TRƯỜNG ĐẠI HỌC SPKT TPHCM CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA ĐIỆN-ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH ĐỘC LẬP - TỰ DO - HẠNH PHÚC Tp Hồ Chí Minh, ngày 10 tháng năm 2021 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Lê Trung Nghĩa Lớp: 15141DT1B MSSV: 15141220 Họ tên sinh viên 2: Nguyễn Lữ Phụng Đình Lớp: 15141DT2A MSSV: 15141130 Tên đề tài: THIẾT KẾ VÀ THI CÔNG XE LĂN TRONG NHÀ TỰ ĐỊNH VỊ VÀ ĐẾN ĐÍCH DỰA VÀO BẢN ĐỒ 2D Xác nhận Nội dung Tuần/ngày GVHD - làm đồ án, tiến hành chọn đồ án (05/10 – 11/10) Gặp giáo viên hướng dẫn để phổ biến yêu cầu - GVHD tiến hành duyệt đề tài - Viết đề cương cho đề tài (12/10 – 18/10) (19/10 – 25/10) (25/10 – 31/10) - Tìm hiểu hoạt động Arduino matlab - Cài đặt matlab, Arduino - Lập trình Arduino với chân I/O để nhúng liệu - Tìm hiểu Rotary Encoder - Thiết lập thuật tốn tính khoảng cách , góc quay dựa vào xung xuất từ encoder - Tìm hiểu cách giao tiếp Arduino với module điều khiển động DC arduino IBT_2 (01/11 – 07/11) - Tìm hiểu cách giao tiếp Arduino với module HC_SC 05 - Tìm hiểu cách giao tiếp sử dụng module LCD cảm ứng Nextion với arduino - Viết lưu đồ chương trình điều khiển động iv an (08/11 – 14/11) - Viết lưu đồ chương trình điều khiển qua Bluetooth cho arduino - Tiến hành viết chương trình hiệu chỉnh chương trình - Tìm hiểu cách kết nối matlab adruino (15/11 – 21/11) - Tiến hành gửi liệu từ Arduino sang Matlab lưu liệu vào excel - Kiểm tra cân chỉnh sai số encoder sai số mơ : chu vi bánh xe , trục xe (22/11 – 28/11) (29/11 – 05/12) 10 (06/12 – 12/12) 11 (13/12 – 19/12) - Chạy thử nghiệm nhiều lần để tìm hệ số xác sửa lỗi định vị - Kết nối module LCD cảm ứng Nextion - Xây dựng thuật toán định vị tự động di chuyển đến địa điểm đích đồ 2D - Kiểm tra, chạy thử sửa lỗi - Vẽ thiết kế mạch in , thiết kế hộp đựng mạch điện - Viết báo cáo - Hoàn thiện sản phẩm , chỉnh sửa báo cáo gửi cho GVHD để xem xét góp ý lần cuối trước in báo cáo 12 (20/12/2020- - Nộp báo cáo làm Slide báo cáo 15/1/2021) GV HƯỚNG DẪN (Ký ghi rõ họ tên) v an 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 an 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 q trình học tập hồ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 an 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 TOÁN VÀ THIẾT KẾ 17 3.1 GIỚI THIỆU 17 3.2 TÍNH TOÁN VÀ THIẾT KẾ HỆ THỐNG 17 3.2.1 Thiết kế sơ đồ khối hệ thống 17 viii an 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 an 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 an 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 an 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 an 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 an 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 an 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 an 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 toán xử lý chưa tối ưu hoàn toà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 tố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 toá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 an 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 an 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 an 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 an 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 an 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 an 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 an 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 an S an K L 0 ... MSSV: 15141130 Tên đề tài: THI? ??T KẾ VÀ THI CÔNG XE LĂN TRONG NHÀ TỰ ĐỊNH VỊ VÀ ĐẾN ĐÍCH DỰA VÀO BẢN ĐỒ 2D Xác nhận Nội dung Tuần/ngày GVHD - làm đồ án, tiến hành chọn đồ án (05/10 – 11/10) Gặp giáo... 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... encoder để định vị điều khiển xe lăn điện đồ 2D kết hợp với điều khiển thị Nextion 1.3 NỘI DUNG NGHIÊN CỨU Mục tiêu xây dựng đề tài: ? ?Thi? ??t kế thi công xe lăn nhà tự định vị đến đích dựa vào đồ 2D? ??