(Đồ án tốt nghiệp) thiết kế và chế tạo robot giám sát an ninh

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO ROBOT GIÁM SÁT AN NINH Người hướng dẫn: TS VÕ NHƯ THÀNH Sinh viên thực hiện: HUỲNH ĐỨC QUANG VINH LÊ ĐỨC THỊNH Đà Nẵng, 2019 ` CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ Độc lập - Tự - Hạnh phúc NHẬN XÉT ĐỒ ÁN TỐT NGHIỆP Thông tin chung: Họ tên sinh viên: Huỳnh Đức Quang Vinh Số thẻ SV: 101150241 Lê Đức Thịnh Số thẻ SV: 101150229 Lớp: 15CDT2 Tên đề tài: Thiết kế chế tạo robot giám sát an ninh Người hướng dẫn: Võ Như Thành Học hàm/ học vị: Tiến sĩ II Nhận xét, đánh giá đồ án tốt nghiệp: Về tính cấp thiết, tính mới, khả ứng dụng đề tài: (điểm tối đa 2đ) ……………………………………………………………………………………… ……………………………………………………………………………………… C C Về kết giải nội dung nhiệm vụ yêu cầu đồ án: (điểm tối đa 4đ) R L T ……………………………………………………………………………………… ……………………………………………………………………………………… Về hình thức, cấu trúc, bố cục đồ án tốt nghiệp: (điểm tối đa 2đ) U D ……………………………………………………………………………………… ……………………………………………………………………………………… Đề tài có giá trị khoa học/ có báo/ giải vấn đề đặt doanh nghiệp nhà trường: (điểm tối đa 1đ) ……………………………………………………………………………………… ……………………………………………………………………………………… Các tồn tại, thiếu sót cần bổ sung, chỉnh sửa: ……………………………………………………………………………………… ……………………………………………………………………………………… III Tinh thần, thái độ làm việc sinh viên: (điểm tối đa 1đ) ……………………………………………………………………………………… IV Đánh giá: Điểm đánh giá: …… /10 (lấy đến số lẻ thập phân) Đề nghị: ☐ Được bảo vệ đồ án ☐ Bổ sung để bảo vệ ☐ Không bảo vệ Đà Nẵng, ngày tháng năm 201 Người hướng dẫn ` ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHẬN XÉT PHẢN BIỆN ĐỒ ÁN TỐT NGHIỆP I Thông tin chung: Họ tên sinh viên: Huỳnh Đức Quang Vinh Lê Đức Thịnh Số thẻ SV: 101150241 Số thẻ SV: 101150229 Lớp: 15CDT2 Tên đề tài: Thiết kế chế tạo robot giám sát an ninh Người phản biện: ………………………….………… Học hàm/ học vị: ………… II Nhận xét, đánh giá đồ án tốt nghiệp: Điểm Điểm TT Các tiêu chí đánh giá tối đa đánh giá Sinh viên có phương pháp nghiên cứu phù hợp, giải đủ nhiệm vụ đồ án giao 80 1a - Tính (nội dung ĐATN có phần so với ĐATN trước đây) - Đề tài có giá trị khoa học, cơng nghệ; ứng dụng thực tiễn 15 1b - Kỹ giải vấn đề; hiểu, vận dụng kiến thức bản, sở, chuyên ngành vấn đề nghiên cứu - Chất lượng nội dung ĐATN (thuyết minh, vẽ, chương trình, mơ hình,…) 50 1c - Có kỹ vận dụng thành thạo phần mềm ứng dụng vấn đề nghiên cứu; - Có kỹ đọc, hiểu tài liệu tiếng nước ứng dụng vấn đề nghiên cứu; - Có kỹ làm việc nhóm; 15 Kỹ viết: 20 2a - Bố cục hợp lý, lập luận rõ ràng, chặt chẽ, lời văn súc tích 15 2b - Thuyết minh đồ án khơng có lỗi tả, in ấn, định dạng C C R L T U D Tổng điểm đánh giá theo thang 100: Quy thang 10 (lấy đến số lẻ) - Các tồn tại, thiếu sót cần bổ sung, chỉnh sửa: ……………………………………… ……………………………………………………………………………………… - Câu hỏi đề nghị sinh viên trả lời buổi bảo vệ: ………………………………… ……………………………………………………………………………………… ` - Đề nghị: ☐ Được bảo vệ đồ án ☐ Bổ sung để bảo vệ ☐ Không bảo vệ Đà Nẵng, ngày tháng năm 201… Người phản biện C C U D R L T ` TÓM TẮT Tên đề tài: Thiết kế chế tạo robot giám sát an ninh Sinh viên thực hiện: Huỳnh Đức Quang Vinh Lê Đức Thịnh Số thẻ SV: 101150241 Số thẻ SV: 101150229 Lớp: 15CDT2 Giảng viên hướng dẫn: TS Võ Như Thành Giảng viên duyệt: TS Lê Hoài Nam Việc giám sát an ninh cho văn phòng tòa nhà quan trọng yêu cầu nguồn nhân lực tài lớn Đặc biệt tịa nhà văn phịng khu cơng nghiệp thường xuyên mục tiêu trộm cắp, phá hoại gián điệp Việc nâng cấp hệ thống bảo mật tiêu tốn phần lớn chi phí tài doanh nghiệp Ngồi ra, với việc sử dụng người việc C C R L T giám sát an ninh có hạn chế định Thay vào đó, sử dụng robot thay cho hiệu suất cao với thời gian hoạt động 24/7, đảm bảo an tồn cho người cắt giảm chi phí cách đáng kể U D Với tiêu chí đặt chúng em chọn thực đề tài: Thiết kế chế tạo robot giám sát an ninh Với mục tiêu tạo sản phẩm hoạt động linh hoạt có khả ứng dụng thực tế mang lại hiệu kinh tế cao ` ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ CỘNG HỊA XÃ HƠI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Huỳnh Đức Quang Vinh Lớp:15CDT2 Khoa:Cơ khí Họ tên sinh viên: Lê Đức Thịnh Lớp:15CDT2 Khoa:Cơ khí Số thẻ sinh viên: 101150241 Ngành: Kỹ thuật Cơ điện tử Số thẻ sinh viên: 101150229 Ngành: Kỹ thuật Cơ điện tử Tên đề tài đồ án: Thiết kế chế tạo robot giám sát an ninh Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu:  Khu vực hoạt động robot: Tầng khu A trường Đại học Bách Khoa  Kích thước tổng thể robot  Các vật liệu: Động cơ, bánh xe, nhựa mica, cảm biến siêu âm, vi điều khiển Nội dung phần thuyết minh tính tốn:  Chương 1: Tổng quan  Chương 2: Tính tốn thiết kế khí  Chương 3: Thiết kế khối điều khiển  Chương 4: Đánh giá đề tài hướng phát triển Các vẽ, đồ thị ( ghi rõ loại kích thước vẽ ):  Bản vẽ tổng thể (1A0)  Bản vẽ sơ đồ động (1A0)  Bản vẽ chi tiết phận (1A0)  Bản vẽ lưu đồ thuật tốn lập trình (1A0)  Bản vẽ sơ đồ mạch điện (1A0) Họ tên người hướng dẫn: TS Võ Như Thành Ngày giao nhiệm vụ đồ án: 5/9/2019 Ngày hoàn thành đồ án: 10/12/2019 Đà Nẵng, ngày 10 tháng 12 năm 2019 Trưởng Bộ môn Người hướng dẫn C C U D R L T ` LỜI NÓI ĐẦU Đầu tiên, em xin chân thành cám ơn tồn thể q thầy thuộc môn Cơ Điện Tử, khoa Cơ Điện Tử giảng dạy cung cấp kiến thức tảng cho em suốt năm học qua Tuy kiến thức chưa đủ để làm việc mơi trường thực tế, lượng kiến thức vơ q báo, tảng để tìm hiểu nghiên cứu sâu hơn, chi tiết vào lĩnh vực Cơ Điện Tử Tiếp theo, em xin chân thành cám ơn thầy Võ Như Thành, thuộc môn Cơ Điện Tử, trường Đại học Bách Khoa Đà Nẵng đã quan tâm giúp đỡ, hướng dẫn hoàn thành tốt đề tài thời gian qua, việc giúp em tìm đưa hướng giải vấn đề Tuy khơng có nhiều thời gian làm dự án xuyên suốt với thầy, nhờ gợi ý thầy mà em thuận lợi giải vấn đề dự án hoàn thành tốt đề tài đồ án môn học Và em chân thành cám ơn trường Đại học Đà Nẵng cung cấp cho em môi trường học tập với đầy đủ trang thiết bị tài liệu học tập nghiên cứu năm học vừa qua C C R L T Cuối em xin chúc trường Đại học Đà Nẵng ngày phát triển mở rộng quy mô, đạt chuẩn chất lượng khu vực giới, cung cấp đầu nguồn nhân lực chất lượng cao Và em xin chúc quý thầy cô thuộc Bộ môn Cơ Điện Tử dồi sức khoẻ thành công sống U D Đà Nẵng, ngày 10 tháng 12 năm 2019 Người thực đề tài Huỳnh Đức Quang Vinh Lê Đức Thịnh i ` CAM ĐOAN Chúng xin cam đoan số liệu kết nghiên cứu đồ án trung thực chưa sử dụng để bảo vệ đồ án học vị khác Mọi giúp đỡ cho việc thực đồ án cảm ơn thơng tin trích dẫn đồ án rõ nguồn gốc phép công bố Sinh viên thực Huỳnh Đức Quang Vinh Lê Đức Thịnh C C R L T U D ii ` MỤC LỤC Tóm tắt Nhiệm vụ đồ án Lời nói đầu cảm ơn i Lời cam đoan ii Mục lục iii Danh sách bảng biểu, hình vẽ iv CHƯƠNG TỔNG QUAN 1.1 Tổng quan robot 1.2 Đặt vấn đề 1.3 Một số nghiên cứu 1.3.1 Robot bảo mật trời Sê-ri XPVII C C 1.3.2 Indoor Security Robot XAT: R L T 1.4 Phạm vi sử dụng: 1.5 Phương pháp tiếp cận: CHƯƠNG 2: TÍNH TỐN VÀ THIẾT KẾ CƠ KHÍ U D 2.1 Yêu cầu thiết kế: 2.2 Phương án thiết kế: 2.2.1 Robot bánh: 2.2.2 Robot bánh xích: 2.2.3 Robot bánh omni: 2.3 Lựa chọn phương án thiết kế: 2.4 Thiết kế khung robot: 2.5 Tính chọn động cơ: 2.5.1 Tính tốn: 2.6 Tính tốn chuyển động robot 2.6.1 Mơ hình robot 2.6.2 Tính tốn động học: 2.6.3 Tính tốn động học cho robot cho số chuyển động bản: 12 iii ` 2.7 Bánh xe đa hướng: 13 CHƯƠNG 3: THIẾT KẾ KHỐI ĐIỀU KHIỂN 14 3.1 Một số dạng điều khiển robot: 14 3.1.1 Điều khiển từ xa tay: 14 3.1.2 Thực thi theo lộ trình: 14 3.1.3 Ngẫu nhiên hoạt động độc lập: 14 3.1.4 Lựa chọn phương án điều khiển: 14 3.2 Sơ đồ khối hệ thống điều khiển: 15 3.3 Một số thành phần bản: 16 3.3.1 STM32F411: 16 3.3.2 Camera: 23 3.3.3 Module điều khiển động L298N 25 C C 3.3.4 Mạch giảm áp LM2596: 26 R L T 3.3.5 Cảm biến siêu âm HC-SR05: 27 3.3.6 Màn hình LCD 20x04: 28 U D 3.3.7 Còi chip(buzzer) 5V: 30 3.3.8 Điện trở A103J: 31 3.4 Encoder: 31 3.5 Bộ điều khiển PID: 32 3.5.1 Giới thiệu điều khiển PID 32 3.5.2 Đặc tính luật điều khiển P,I,D 33 3.6 Lưu đồ thuật toán điều khiển: 34 3.7 Sơ đồ mạch điện: 35 CHƯƠNG 4: ĐÁNH GIÁ ĐỀ TÀI VÀ HƯỚNG PHÁT TRIỂN 36 4.1 Kết đạt được: 36 4.2 Hạn chế tồn tại: 37 4.3 Hướng phát triển đề tài 37 Tài liệu tham khảo 38 PHỤ LỤC 39 iv Thiết kế chế tạo robot giám sát an ninh Thường encoder tương đối có kênh gồm kênh A, kênh B, kênh I Kênh I dùng để đếm số vòng quay đĩa, kênh A kênh B lệch pha 900 Bằng cách phối hợp kênh A B, ta biết chiều quay động Encoder tuyệt đối : hạn chế encoder tương đối số xung phải đếm lưu đệm đếm ngoài, nguồn, giá trị đếm bị Trong trường hợp thiết bị cần tắt nguồn định kỳ để bảo dưỡng cấp nguồn lại khơng biết tiếp tục vị trí Encoder tuyệt đối khắc phục điều thiết kế đĩa với vòng đồng tâm đĩa, vòng có chỗ hở cho ánh sáng qua chỗ kín xen kẽ gọi phần tử nhị phân Các vòng bắt nguồn từ tâm đĩa xa số phần tử nhị phân lại tăng gấp đơi Giá trị đọc có dạng số nhị phân cho vị trí tương đương đĩa Đồ án sử dụng Sharp Rotary Encoder Đây encoder tương đối có 256 xung vòng Các dây encoder : C C  Dây đỏ : dây nguồn +5V cho encoder R L T  Dây xanh da trời : dây đất cho encoder  Dây nâu : tín hiệu kênh A cho encoder  Dây vàng : tín hiệu kênh B cho encoder U D Ở sử dụng dây : dây đỏ, dây xanh da trời dây nâu 3.5 Bộ điều khiển PID: 3.5.1 Giới thiệu điều khiển PID Một điều khiển vi tích phân tỉ lệ (bộ điều khiển PID) chế phản hồi vòng điều khiển tổng quát sử dụng rộng rãi hệ thống điều khiển công nghiệp- điều khiển PID sử dụng phổ biến số điều khiển có phản hồi Một điều khiển PID tính tốn giá trị ‘sai số’ hiệu số giá trị thông số đo giá trị mong muốn đạt Bộ điều khiển thực giảm tối đa sai số cách điều chỉnh giá trị điều khiển đầu vào Trong trường hợp khơng có kiến thức trình, điều khiển PID lựa chọn tốt Tuy nhiên, để đạt kết tốt nhất, thơng số PID sử dụng tính tốn phải điều chỉnh theo tính chất hệ thống- kiểu điều khiển giống nhau, thông số phải phụ thuộc vào đặc thù hệ thống Một điều khiển PID bao gồm khâu: khâu tỷ lệ (P), khâu tích phân (I), khâu vi phân (D) Trong miền thời gian, điều khiển PID mơ tả mơ hình vào ra: SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 32 Thiết kế chế tạo robot giám sát an ninh t u  t   K p e  t   K1  e  t dt  K D de  t  dt Trong u(t) tín hiệu ngõ vào, e(t) tín hiệu ngõ điều khiển Tín hiệu u gửi tới cấu chấp hành, cho đầu y Đầu y gửi đến cảm biến đo ta lại có giá trị sai số e Bộ điều khiển lại lấy giá trị sai số mới, tính tốn tích phân, vi phân lại đưa tín hiệu điều khiển Quá trình lặp lặp lại C C R L T Hình 3.20: Mơ hình điều khiển PID 3.5.2 Đặc tính luật điều khiển P,I,D U D Luật điều khiển P có tác dụng làm giảm thời gian xác lập khử sai số trạng thái xác lập Luật điều khiển I có tác dụng khử sai số trạng thái, làm giảm khả đáp ứng tức thời hệ thống Luật điều khiển D có tác dụng tăng độ ổn định hệ thống giảm độ điều chỉnh, cải thiện khả đáp ứng tức thời Hiệu luật điều khiển P,I D lên hệ thống vịng kín tổng kết qua bảng sau: Thông số Thời gian độ Độ điều chỉnh Thời gian đến ổn định Sai số tĩnh Kp Giảm Tăng Ít thay đổi Giảm Ki Giảm Tăng Tăng Khử Kd Ít thay đổi Giảm Giảm Ít thay đổi SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 33 Thiết kế chế tạo robot giám sát an ninh 3.6 Lưu đồ thuật toán điều khiển: C C R L T U D Hình 3.21: Lưu đồ thuật tốn điều khiển SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 34 Thiết kế chế tạo robot giám sát an ninh 3.7 Sơ đồ mạch điện: C C R L T U D Hình 3.22: Sơ đồ mạch điện điều khiển robot SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 35 Thiết kế chế tạo robot giám sát an ninh CHƯƠNG 4: ĐÁNH GIÁ ĐỀ TÀI VÀ HƯỚNG PHÁT TRIỂN 4.1 Kết đạt được: Qua trình hiểu, nghiên cứu để tìm mơ hình thích hợp, phương pháp tối ưu, nghiên cứu phần cứng phần mềm để hồn thiện mơ hình chúng em thu nhiều kinh nghiệm quý báu, chúng em hiểu sâu rộng loại robot tuần tra, cấu trúc robot, phận, thiết bị, phần mềm lập trình điều khiển robot Mặc dù q trình hồn thiện sản phẩm chúng em gặp nhiều khó khăn việc tìm thiết bị phù hợp tương ứng với mơ hình thực hiện, đầu tư kinh phí, việc lắp ráp khối lại với Nhưng chúng em hoàn thiện với thời gian quy định, hệ thống hoạt động với yêu cầu đề C C R L T U D Hình 4.1: Hình ảnh mạch thực tế SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 36 Thiết kế chế tạo robot giám sát an ninh C C R L T Hình 4.2: Mơ hình thực tế U D 4.2 Hạn chế tồn tại:  Robot hoạt động chưa mượt mà, hiệu suất đạt chưa cao  Robot hoạt động phạm vi nhỏ  Thời gian hoạt động robot hạn chế, chưa đáp ứng nhu cầu thực tế  Phân chia công việc nhóm chưa thực phù hợp 4.3 Hướng phát triển đề tài  Robot hoạt động độc lập, nhận giám sát người tảng phần mềm  Thiết lập la bàn cho robot tự hoạt động phạm vi lớn điều kiện khó khăn  Kết hợp xử lý khuôn mặt để nhận diện người vật thể khác  Phát triển cung cấp liệu data pin cho người dùng  Có chức Home tự sạc pin sau phiên làm việc SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 37 Thiết kế chế tạo robot giám sát an ninh Tài liệu tham khảo [1] https://www.xinhengjiarobotics.com/indoor-security-robot-xat/ [2] https://www.xinhengjiarobotics.com/xpvii/ [3] https://www.robotshop.com/uk/4-wheels-scout-platform-robot-kit.html [4]http://www.andcircuit.com/2017/06/19/crawler-robot-tank-chassis-metaltracked-crawler-smart-car-diy/ [5] http://www.dagurobot.com/Omni_Wheel_Chassis [6] https://robothutbui.vn/robot-hut-bui-irobot [7] https://vietnamese.alibaba.com/product-detail/newest-4th-generation-robomowrs630-lawn-mower-robot-lawn-mower1974136666.html?spm=a2700.8699010.normalList.19.b31b26dfLq2jOr C C [8] Byung-JuYi, Whee KukKim, Kinematic Modeling of Omni-Directional Mobile Robots as Parallel Manipulator R L T https://www.sciencedirect.com/science/article/pii/S1474667017342908 U D SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 38 Thiết kế chế tạo robot giám sát an ninh PHỤ LỤC Lập trình chương trình cho Robot: #include #include #include #include #include "UsbSerial.h" #include "Trackbar.h" #include "Delay.h" #include "Gpio.h" #include "SonarSensor.h" #include "UpbitCounter.h" C C #include "Lcd.h" R L T #include "MotorPid.h" #include "Schedule.h" #include "Button.h" U D #define NUM_SONAR constexpr float WHEEL_RADIUS = 58 / 2.0f; constexpr float WHEEL_SPAN_RADIUS = 145; constexpr float runSpeed = 15; Button btnUp, btnDown, btnLeft, btnRight; SonarSensor sonar[4] = { SonarSensor(Gpio(TRIG1_GPIO_Port, TRIG1_Pin), Gpio(ECHO1_GPIO_Port, ECHO1_Pin)), SonarSensor(Gpio(TRIG2_GPIO_Port, TRIG2_Pin), Gpio(ECHO2_GPIO_Port, ECHO2_Pin)), SonarSensor(Gpio(TRIG3_GPIO_Port, TRIG3_Pin), Gpio(ECHO3_GPIO_Port, ECHO3_Pin)), SonarSensor(Gpio(TRIG4_GPIO_Port, TRIG4_Pin), Gpio(ECHO4_GPIO_Port, ECHO4_Pin))}; UpbitCounter encoderA; // timer 2, 32bit UpbitCounter encoderB; // timer 1, 16bit UpbitCounter encoderC; // timer 3, 16bit SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 39 Thiết kế chế tạo robot giám sát an ninh LCD lcd = LCD(Gpio(LCD_RS_GPIO_Port, LCD_RS_Pin), Gpio(LCD_E_GPIO_Port, LCD_E_Pin), Gpio(LCD_D4_GPIO_Port, LCD_D4_Pin), Gpio(LCD_D5_GPIO_Port, LCD_D5_Pin), Gpio(LCD_D6_GPIO_Port, LCD_D6_Pin), Gpio(LCD_D7_GPIO_Port, LCD_D7_Pin)); extern TIM_HandleTypeDef htim9, htim10; MotorPid motorA = MotorPid(Gpio(DIR1A_GPIO_Port, DIR1A_Pin), Gpio(DIR1B_GPIO_Port, DIR1B_Pin), htim9, TIM_CHANNEL_1); MotorPid motorB = MotorPid(Gpio(DIR2A_GPIO_Port, DIR2A_Pin), Gpio(DIR2B_GPIO_Port, DIR2B_Pin), C C htim9, TIM_CHANNEL_2); R L T MotorPid motorC = MotorPid(Gpio(DIR3A_GPIO_Port, DIR3A_Pin), Gpio(DIR3B_GPIO_Port, DIR3B_Pin), htim10, TIM_CHANNEL_1); U D enum class RunStage : uint8_t { Stop, RunStraightAlignLeft, RunStraightAlightRight, RunStraightWaitToTurn, TurningLeft, RunStraightEscapeVoid }; RunStage runStage = RunStage::Stop; uint32_t lastStageChange_ms = 0; void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { for (uint8_t i = 0; i < NUM_SONAR; ++i) sonar[i].onEchoChangedInterrupt(); } SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 40 Thiết kế chế tạo robot giám sát an ninh // calculate wheels' velocity void move(float vx, float vy, float rotation) { rotation *= WHEEL_SPAN_RADIUS; float omega1 = (-0.333f * vx + 0.58f * vy + 0.333f * rotation) / WHEEL_RADIUS; float omega2 = (-0.333f * vx - 0.58f * vy + 0.333f * rotation) / WHEEL_RADIUS; float omega3 = (0.667f * vx + 0.333f * rotation) / WHEEL_RADIUS; motorA.moveSpeed(-omega3); motorB.moveSpeed(omega2); motorC.moveSpeed(omega1); } C C bool isMoving() R L T { return motorA.isRunning() && motorB.isRunning() && motorC.isRunning(); } U D double vx, vy, rotation; Trackbar tbMotorKp("MP", vx), tbMotorKi("MI", vy), tbMotorKd("MD", rotation); // string to upper case void strupr(char *str) { char *s = str; while (*s) { *s = toupper((unsigned char)*s); s++; } } void handleGUI() { if (Serial.available() == 0) SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 41 Thiết kế chế tạo robot giám sát an ninh return; char message[64]; Serial.readBytes((uint8_t *)message, sizeof(message)); strupr(message); char rw = message[0]; char cmdName[16]; float value; if (rw != 'R' && rw != 'W') { Serial.println("ERROR"); return; } sscanf(message, "%c %s %f", &rw, cmdName, &value); C C bool foundTrackbar = false; R L T // thay doi trackbar for (uint8_t i = 0; i < Trackbar::instancesCount; ++i) { U D if (strcmp(cmdName, Trackbar::instances[i]->key) == 0) { if (rw == 'R') value = Trackbar::instances[i]->value; if (rw == 'W') Trackbar::instances[i]->value = value; foundTrackbar = true; break; } } if (!foundTrackbar) { Serial.println("NOT FOUND"); return; } Serial.printf("%s %9.4f\n", cmdName, value); SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 42 Thiết kế chế tạo robot giám sát an ninh move(vx, vy, rotation); } extern void hwEncoderInit(); void setup() { Serial.begin(); hwEncoderInit(); lcd.init(20, 4); lcd.print("Welcome!"); motorA.init(); motorB.init(); motorC.init(); for (uint8_t i = 0; i < NUM_SONAR; ++i) C C sonar[i].init(); R L T motorA.setAccel(1); motorA.setPeakVelocity(2); motorB.setAccel(1); U D motorB.setPeakVelocity(2); motorC.setAccel(1); motorC.setPeakVelocity(2); motorA.moveSpeed(0); motorB.moveSpeed(0); motorC.moveSpeed(0); btnUp.attach(Gpio(BTN1_GPIO_Port, BTN1_Pin)); btnLeft.attach(Gpio(BTN2_GPIO_Port, BTN2_Pin)); btnRight.attach(Gpio(BTN3_GPIO_Port, BTN3_Pin)); btnDown.attach(Gpio(BTN4_GPIO_Port, BTN4_Pin)); btnUp.onButtonPressed = []() { Serial.println("btnUp clicked"); vx = 0; vy = runSpeed; rotation = 0; runStage = RunStage::RunStraightAlignLeft; SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 43 Thiết kế chế tạo robot giám sát an ninh }; btnLeft.onButtonPressed = []() { Serial.println("btnLeft clicked"); }; btnRight.onButtonPressed = []() { Serial.println("btnRight clicked"); }; btnDown.onButtonPressed = []() { Serial.println("btnDown clicked"); vx = 0; vy = 0; rotation = 0; runStage = RunStage::Stop; C C }; R L T } void loop() { U D const uint32_t current_ms = HAL_GetTick(); handleGUI(); btnUp.update(); btnLeft.update(); btnRight.update(); btnDown.update(); encoderA.update(TIM2->CNT); encoderB.update(TIM1->CNT); encoderC.update(TIM3->CNT); motorA.update(encoderA.getCount()); motorB.update(encoderB.getCount()); motorC.update(encoderC.getCount()); SCHEDULER_CREATE(lastMeasureSonar_ms); if (onSchedule(current_ms, lastMeasureSonar_ms, NUM_SONAR * 25 + 30)) { for (uint8_t i = 0; i < NUM_SONAR; ++i) SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 44 Thiết kế chế tạo robot giám sát an ninh sonar[i].startMeasure(25000ul * i); } for (uint8_t i = 0; i < NUM_SONAR; ++i) sonar[i].update() /* SCHEDULER_CREATE(lastSerialPrint_ms); if (onSchedule(current_ms, lastSerialPrint_ms, 50)) { Serial.printf("%4.0f %4.0f %4.0f %4.0f\n", sonar[0].getFiltedDistace_mm(), sonar[1].getFiltedDistace_mm(), sonar[2].getFiltedDistace_mm(), sonar[3].getFiltedDistace_mm()); // Serial.printf("%ld %ld %ld\n", encoderA.getCount(), encoderB.getCount(), encoderC.getCount()); } */ C C SCHEDULER_CREATE(lastLcdUpdate_ms); if (onSchedule(current_ms, lastLcdUpdate_ms, 200)) R L T { lcd.clear(); lcd.printf("%4.0f %4.0f %4.0f sonar[1].getFiltedDistace_mm(), U D %4.0f\n", sonar[0].getFiltedDistace_mm(), sonar[2].getFiltedDistace_mm(), sonar[3].getFiltedDistace_mm()); lcd.setCursor(0, 1); char text[21]; switch (runStage) { case RunStage::Stop: strncpy(text, "Stop", sizeof text); break; case RunStage::RunStraightAlignLeft: strncpy(text, "Align Left", sizeof text); break; case RunStage::RunStraightAlightRight: strncpy(text, "Align Right", sizeof text); break; SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 45 Thiết kế chế tạo robot giám sát an ninh case RunStage::RunStraightWaitToTurn: strncpy(text, "Wait turn", sizeof text); break; case RunStage::TurningLeft: strncpy(text, "Turning Left", sizeof text); break; case RunStage::RunStraightEscapeVoid: strncpy(text, "Wait escape void", sizeof text); break; default: break; } lcd.print(text); C C lcd.setCursor(0, 3); R L T lcd.printf("vx %3.1f vy %3.1f rt %3.1f", vx, vy, rotation); } SCHEDULER_CREATE(lastUpdateMove_ms); switch (runStage) { U D case RunStage::Stop: move(0, 0, 0); break; case RunStage::RunStraightAlignLeft: { if (onSchedule(current_ms, lastUpdateMove_ms, 100)) { move(vx, vy, rotation); } break; } default: break; }} SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 46 ... Hình 2.3: Robot bánh omni [5] SVTH: Huỳnh Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành Thiết kế chế tạo robot giám sát an ninh 2.3 Lựa chọn phương án thiết kế: Với yêu cầu đặt robot di... Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành Thiết kế chế tạo robot giám sát an ninh 2.6 Tính tốn chuyển động robot 2.6.1 Mơ hình robot Chuyển động robot chuyển động bánh xe Các bánh xe... Đức Quang Vinh – Lê Đức Thịnh GVHD: TS Võ Như Thành 12 Thiết kế chế tạo robot giám sát an ninh 2.7 Bánh xe đa hướng: a Giới thiệu bánh xe đa hướng: Bánh xe đa hướng omni loại bánh có bánh vệ