(LUẬN văn THẠC sĩ) nghiên cứu, thiết kế mạch điều khiển động cơ ứng dụng cho robot rhex luận văn ths kỹ thuật điện, điện tử và viễn thông 85103

1 ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ LÂM VŨ NGHIÊN CỨU, THIẾT KẾ MẠCH ĐIỀU KHIỂN ĐỘNG CƠ ỨNG DỤNG CHO ROBOT RHEX LUẬN VĂN THẠC SĨ CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ, TRUYỀN THÔNG HÀ NỘI – Năm 2018 TIEU LUAN MOI download : skknchat@gmail.com ĐẠI HỌC QUỐC GIA HÀ NỘI LỜI CAM ĐOAN TRƯỜNG ĐẠI HỌC CƠNG NGHỆ Trong q trình làm luận văn, đọc tham khảo nhiều tài liệu từ giáo trình, sách chun mơn nhiều báo đăng tải LÂM VŨ NGHIÊN CỨU, THIẾT KẾ MẠCH ĐIỀU KHIỂN ĐỘNG CƠ ỨNG DỤNG CHO ROBOT RHEX Ngành: Công nghệ kỹ thuật điện tử, truyền thông Chuyên ngành: Kỹ thuật điện tử Mã số: 8510302.01 LUẬN VĂN THẠC SĨ CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ, TRUYỀN THÔNG NGƯỜI HƯỚNG DẪN KHOA HỌC: TRUNG TÁ.TS VŨ VĂN TÂM PGS TS TRẦN ĐỨC TÂN HÀ NỘI – Năm 2018 TIEU LUAN MOI download : skknchat@gmail.com i LỜI CAM ĐOAN Tôi xin cam đoan tơi viết hồn tồn thống, chân thực, kết đo đạc thực nghiệm đạt khóa luận khơng chép từ tài liệu hình thức Những kết tơi nghiên cứu, tích lũy q trình làm luận văn Tơi hồn tồn chịu trách nhiệm có dấu hiệu chép kết từ tài liệu khác Hà Nội, ngày tháng năm 2018 TÁC GIẢ LÂM VŨ TIEU LUAN MOI download : skknchat@gmail.com ii LỜI CẢM ƠN Trong thời gian nghiên cứu hoàn thiện luận văn nhận giúp đỡ chu đáo Thầy, Cô giáo khoa Điện tử - Viễn thông, Trường Đại Học Công Nghệ, Đại Học Quốc Gia Hà Nội Đề tài:” Nghiên cứu, thiết kế mạch điều khiển động ứng dụng cho Robot Rhex” triển khai thực hoàn thành với số kết thu ứng dụng điều kiện thực tiễn Tơi xin bày tỏ lịng biết ơn sâu sắc đến thầy Trần Đức Tân, thầy Vũ Văn Tâm trực tiếp hướng dẫn trình nghiên cứu thực đề tài với tất lịng nhiệt tình chu đáo, ân cần với thái độ nghiên cứu khoa học nghiêm túc thẳng thắn nhà khoa học uy tín, mẫu mực Tôi xin chân thành cảm ơn thầy cô giáo, anh chị bạn có đóng góp ý kịp thời bổ ích, giúp đỡ suốt q trình nghiên cứu hồn thiện khóa luận Mặc dù có nhiều cố gắng hồn thiện luận văn tất nhiệt tình nỗ lực mình, nhiên khơng thể tránh khỏi thiếu sót, mong nhận ý kiến đóng góp q báu q Thầy Cơ bạn TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 iii MỤC LỤC LỜI CAM ĐOAN i LỜI CẢM ƠN ii DANH MỤC HÌNH VẼ v DANH MỤC BẢNG BIỂU vii MỞ ĐẦU 1 Tính cấp thiết đề tài Mục đích nghiên cứu Đối tượng nghiên cứu Phương pháp nghiên cứu Ý nghĩa khoa học thực tiễn đề tài Kết cấu luận văn Chương TỔNG QUAN VỀ ROBOT RHEX 1.1 Giới thiệu chung robot Rhex 1.2 Sự phát triển Robot Rhex 1.3 Một số phương pháp điều khiển động robot Rhex 1.3.1 Khái niệm chung 1.3.2 Cấu tạo động điện chiều 1.3.3 Nguyên lý làm việc động điện chiều 1.3.4 Phân loại động chiều 1.3.5 Điều chỉnh tốc độ động sử dụng chỉnh lưu bán dẫn 1.3.6 Điều chỉnh tốc độ động sử dụng thiết bị điều chỉnh xung áp 1.3.7 Một số phương pháp điều khiển tốc độ chiều quay dùng Transistor Thyristor 10 1.3.8 Phương án dùng vi điều khiển 13 1.4 Kết luận 15 Chương 16 XÂY DỰNG HỆ THỐNG ĐIỀU KHIỂN ĐỘNG CƠ 16 2.1 Yêu cầu xây dựng hệ thống điều khiển động 16 2.2 Sơ đồ điều khiển động 16 2.2.1 Sơ đồ khối 16 2.2.2 Sơ đồ nguyên lý 17 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 iv 2.2.3 Lưu đồ thuật toán 18 2.3 Phương pháp điều khiển động 19 2.3.1 Arduino phát PWM điều chỉnh vận tốc động 19 2.3.2 Giải thuật điều khiển vi tích phân tỉ lệ (PID) 20 2.4 Lựa chọn thiết bị điều khiển trung tâm 25 2.4.1 Tổng quan Arduino 25 2.4.14 Các linh kiện khác board Arduino ATMega 2560 27 2.4.2 Thông số kỹ thuật Arduino ATMega 2560 27 2.4.3 Lập trình cho Arduino 31 2.5 Lựa chọn mạch Driver 33 2.6 Lựa chọn động DC 36 Chương 43 KẾT QUẢ VÀ THỬ NGHIỆM 43 3.1 Kết 43 3.2 Thử nghiệm 44 3.2.1 Thử nghiệm thao tác vịng trái, vịng phải với góc độ khác 45 3.2.2 Thử nghiệm Robot di chuyển địa hình phẳng 47 3.2.3 Thử nghiệm Robot lên, xuống bậc cao 48 KẾT LUẬN 50 Kết nghiên cứu đề tài 50 Hướng phát triển đề tài: 50 TÀI LIỆU THAM KHẢO 52 PHỤ LỤC 52 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 v DANH MỤC HÌNH VẼ Hình 1.1 Robot Rhex Hình 1.2 Điều chỉnh điện áp phần ứng chỉnh lưu có điều khiển Hình 1.3 Biểu đồ thời gian điện áp dòng điện Hình 1.4 Biểu đồ thời gian điện áp dịng điện Hình 1.5 Sơ đồ nguyên lý mạch điều khiển tốc độ động chiều có đảo chiều quay 11 Hình 1.6 Sơ đồ nguyên lý mạch đảo chiều động dùng Transistor khuếch đại thuật toán 11 Hình 1.7 Sơ đồ điều chỉnh tốc độ động chiều dùng điôt Thyristor 13 Hình 1.8 Sơ đồ mạch điều khiển động cho chân số Robot 14 Hình 2.1 Sơ đồ khối trình điều khiển động 16 Hình 2.2 Sơ đồ nguyên lý bao gồm Arduino mega 2560, IC driver LM 298, động DC 17 Hình 2.3 Lưu đồ thuật tốn 18 Hình 2.4 Giản đồ thời gian xung PWM 19 Hình 2.5 Sơ đồ khối giải thuật P 20 Hình 2.6 Sơ đồ khối ứng dụng giải thuật PID vào robot Rhex 22 Hình 2.7 Lưu đồ giải thuật hiệu chỉnh PID 25 Hình 2.8 Arduino Mega 2560 28 Hình 2.9 Mạch điều khiển máy in 3D RAMPS 1.4 29 Hình 2.10 Board mạch mở rộng cho Arduino Mega 256 30 Hình 2.11 Ứng dụng Arduino với IoT 31 Hình 2.12 Mơi trường lập trình Arduino 32 Hình 2.13 Sơ đồ khối IC LM 298 33 Hình 2.14 IC LM 298 .34 Hình 2.15 Sơ đồ màu dây động DC 36 Hình 2.16 Hình ảnh thực tế động 37 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 vi Hình 2.17 Optical Encoder .38 Hình 2.18 Hai kênh A B lệch pha encoder 39 Hình 2.17 Cảm biến la bàn số QMC5883L .40 Hình 2.19 Cảm biến la bàn số QMC5883L 41 Hình 3.1 Bản vẽ mạch in hai lớp 43 Hình 3.2 Mạch điện thực tế 43 Hình 3.3 Lắp ráp bo mạch vào robot Rhex 44 Hình 3.4 Đồ thị hiển thị góc quay 46 Hình 3.5 Thử nghiệm cho Robot chỗ vòng trái, vòng phải 46 Hình 3.6 Đồ thị hiển thị tín hiệu di chuyển đường phẳng 47 Hình 3.7 Thử nghiệm Robot di chuyển đường phẳng 47 Hình 3.8 Đồ thị hiển thị tín hiệu lên xuống bậc cao 48 Hình 3.9 Thử nghiệm Robot lên xuống bậc cao 49 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 vii DANH MỤC BẢNG BIỂU Bảng 2.1 Luật điều khiển PID 21 Bảng 2.2 Bốn cổng nối tiếp giao tiếp với phần cứng .29 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 MỞ ĐẦU Tính cấp thiết đề tài Trên giới, có nhiều loại robot chế tạo, đưa vào sử dụng nhằm mục đích phục vụ, thay thế, giải phóng sức lao động người Có nhiều lĩnh vực khoa học, kĩ thuật đời sống bắt buộc phải có robot vào làm việc Ở Việt Nam năm gần đây, với phát triển không ngừng khoa học, kỹ thuật công nghệ, robot xâm nhập vào đời sống chúng ta, thực trợ thủ đắc lực giúp người giải quyết, xử lý công việc phức tạp, có tính chun mơn hóa cao Trong lĩnh vực an ninh, quốc phòng, việc phát minh, sáng chế chế tạo robot thay chiến sĩ cảnh khuyển thực trở thành ngành khoa học quân Tình hình tội phạm nước diễn biến phức tạp nguy hiểm, đặc biệt tội phạm ma túy, khủng bố Chúng thường cố thủ, liều lĩnh làm tăng tính rủi ro cho cán bộ, chiến sĩ thực thi nhiệm vụ Xuất phát từ lý luận thực tiễn trên, mạnh dạn chọn đề tài: Nghiên cứu, thiết kế mạch điều khiển động ứng dụng cho Robot Rhex Đề tài tập trung vào việc nghiên cứu, thiết kế hệ thống mạch điều khiển động chân robot Mục đích nghiên cứu Đề tài tiến hành tìm hiểu nguyên lý hoạt động robot nghiên cứu phương pháp chế tạo mạch điều khiển động chân robot để nâng cao hiệu robot di chuyển Phân tích so sánh, đánh giá phương pháp, sử dụng (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 45 3.2 Thử nghiệm 3.2.1 Thử nghiệm thao tác vòng trái, vịng phải với góc độ khác Hàm truyền đạt động DC robot thực thao tác vịng trái, vịng phải: (3.1) Xác định thơng số điều khiển Bộ điều khiển PID ( KD =0 ) KP = 0,6 TN = 0,5Tcrit = 0,021 TV = 0,125Tcrit = 0,00525 KI = 1318,29 KD = KP.TV = 0,145 Chỉnh thông số KP, KI, KD tương ứng với điều khiển ta đồ thị: (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 46 v(cm/s) t(s) Hình 3.2 Đồ thị hiển thị tín hiệu góc quay Khi Robot đứng (khơng di chuyển tiến lùi), người sử dụng điều khiển Robot vịng trái, vịng phải với góc 45 độ, 90 độ, 180 độ Trong thực tế, thao tác thường sử dụng Robot cần phải rẽ hướng địa hình phức tạp (khơng thể vừa di chuyển tiến, lùi vừa rẽ hướng được) + Đánh giá kết quả: a) Robot vòng phải b) Robot vịng trái Hình 3.3 Thử nghiệm cho Robot chỗ vòng phải, vòng trái (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 47 - Ưu điểm: Do có phương pháp di chuyển khác với di chuyển thơng thường, nên q trình vịng phải, vòng trái linh hoạt; Robot chuyển hướng theo hướng điều khiển, đặc biệt rẽ vng góc Vị trí Robot sau rẽ hướng không thay đổi so với trước rẽ - Hạn chế: Góc rẽ chưa xác, phụ thuộc vào địa hình nên chân Robot bị vướng làm sai góc rẽ (thơng thường góc rẽ < góc cần rẽ) 3.2.2 Thử nghiệm Robot di chuyển địa hình phẳng Cho KI = KD = 0, KP = 0,5 v (cm/s) t(s) Hình 3.4 Đồ thị hiển thị tín hiệu di chuyển phẳng + Thử nghiệm địa hình tương đối phẳng Khi di chuyển đường phẳng, Robot sử dụng phương pháp di chuyển thông thường (từng cặp chân thay chuyển động) Hình 3.5 Thử nghiệm Robot di chuyển đường (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 48 Đây phương pháp di chuyển chủ yếu, sử dụng nhiều Để đảm bảo Robot khơng bị lệch hướng tốc độ, thời điểm chạm đất chân phải đồng với - Ưu điểm: Việc di chuyển (tiến, lùi; thực vòng trái, vòng phải tiến) Robot thực hiên tương đối tốt Các chân Robot đồng với nhau, Robot không bị giật chuyển bước Khi đứng lại Robot trở trạng thái yêu cầu (trạng thái sẵn sàng di chuyển cho bước tiếp theo) Việc điều khiển tiến, lùi, vòng trái, vòng phải, tạm dừng có hiệu tức (khi kết thúc bước thực Robot chuyển trạng thái yêu cầu) Khi di chuyển đường bằng, tốc độ Robot đặt mức đến mức - Hạn chế: Khi đặt tốc độ di chuyển mức (mức cao nhất) Robot có cảm giác bị giật (do tốc độ nhanh, cảm biến không cập nhật kịp) [1] 3.2.3 Thử nghiệm Robot lên, xuống bậc cao Cho KP = KI = 2; KD = 0,01 ta có đồ thị sau: v (cm/s) t(s) Hình 3.6 Đồ thị hiển thị tín hiệu lên xuống bậc cao (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 49 Theo thiết kế, Robot có khả lên, xuống bậc thẳng đứng có chiều cao < lần đường kính chân Robot Với đường kính chân 20 cm, nhóm nghiên cứu đề tài thử nghiệm bậc cao 35 cm - Ưu điểm: Đối với bậc cao < 40 cm Robot có khả leo lên, leo xuống tốt; đồng thời Robot không bị lật Các cảm biến có phản ứng tốt, Robot bắt đầu bước chân lên bậc (tăng tốc độ để tăng lực đẩy); đồng thời Robot tự điều chỉnh để đảm bảo thăng lên bậc xuống bậc Khi hồn thành việc lên, xuống bậc Robot lại trở trạng thái di chuyển bình thường - Hạn chế: Khi Pin yếu tốc độ đặt mức thấp việc leo bậc gặp khó khăn (Robot phải dừng lại để tăng tốc nhiều lần) Khi leo bậc mà góc chạm Robot với bề thành đứng bậc thấp (< 30 độ) Robot bị lệch hướng [1] Hình 3.7 Thử nghiệm Robot lên, xuống bậc cao (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 50 KẾT LUẬN Kết nghiên cứu đề tài Về sở lý thuyết: - Phân tích lựa chọn phương án thiết kế hợp lý - Phân tích tính tốn thơng số kỹ thuật cần thiết - Sử dụng phương pháp điều khiển tối ưu hóa cho robot Về mặt thực nghiệm: - Chế tạo thành công hệ thống điều khiển động cho robot Áp dụng thực tiễn: - Với kết đạt ứng dụng đề tài vào việc điều khiển robot từ xa sóng vơ tuyến thu hình ảnh trực quan từ camera robot máy tính - Đề tài góp phần làm phát triển thêm hình thức mơ hình, mơ cho robot địa hình chun dụng có khả thay hoạt động người khu vực nguy hiểm - Đề tài góp phần phục vụ cho công việc học tập, nghiên cứu sinh viên ngành khí, tự động hóa… Hướng phát triển đề tài: Đề tài có tính mở, có nhiều vấn đề tiếp tục triển khai nghiên cứu phát triển đề tài sau: - Hoàn thiện kết cấu khí để robot hoạt động hiệu địa hình phức tạp (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 51 - Hướng đến phát triển việc điều khiển robot khoảng cách xa qua đường truyền Internet - Định vị cho robot [13,15] - Trang bị cho robot cảm biến đo nhiệt độ, độ ẩm, độ phóng xạ… để gửi tín hiệu trạng thái vùng hoạt động robot máy tính, làm tăng thêm khả ứng dụng cho robot - Trang bị thêm tay máy gắn robot để người điều khiển thực thao tác mong muốn từ xa - Xây dựng, hoàn thiện khả làm việc nhóm robot tạo mạnh bật so với loại robot khác… (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 52 TÀI LIỆU THAM KHẢO Tiếng Việt Trung tá, TS Vũ Văn Tâm (2018), Nghiên cứu thiết kế chế tạo robot chuyên dùng phục vụ lực lượng công an nhân dân, trường Đại học Kỹ thuật – Hậu cần CAND Đặng Công Huy Minh (2013), Thiết kế chế tạo robot vượt địa hình phức tạp, luận văn thạc sĩ trường Đại học Đà Nẵng Phạm Quang Huy, Lê CảnhTrung, Lập trình điều khiển với Arduino, Nhà xuất khoa học kỹ thuật TS Lê Mỹ Hà, KS Phạm Quang Huy, Lập trình IoT với Arduino, Nhà xuất Thanh niên Phạm Quang Huy, Lê Cảnh Trung, Bài tập thực hành Arduino - Lập trình điều khiển với Arduino, Nhà xuất khoa học kỹ thuật TS Trương Đình Nhơn, KS Phạm Quang Huy, Hướng dẫn sử dụng Arduino, Nhà xuất Thanh niên Tiếng Anh Kevin C Galloway, Galen Clark Haynes, B Deniz Ilhan, Aaron M Johnson, Ryan Knopf (2010), “X-RHex: A Highly Mobile Hexapedal Robot for SensoriMotor Tasks”, University of Pennsylvania Scholarly Commons Department of Electrical & Systems Engineering Joel D.Weingarten, Gabriel A D Lopes, Martin Buehler, Richard E Grof, Daniel E Koditschek (2004), ”Automated Gait Adaptation for Legged Robots”,Reprinted from Proceedings of the 2004 IEEE International Conference on Robotics and Automation (ICR 2004), Volume 3, pages 2153-2158 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 53 Z Moore and M Buehler (2001),“Stable Stair Climbing in a Simple Hexapod Robot”, Ambulatory Robotic Lab, Centre for Intelligent Machines, McGill University 10 Joel D.Weingarten, Gabriel A D Lopes, Martin Buehler, Richard E Grof, Daniel E Koditschek (2004), ”Automated Gait Adaptation for Legged Robots”, Reprinted from Proceedings of the 2004 IEEE International Conference on Robotics and Automation (ICR 2004), Volume 3, pages 2153-2158 11 Uluc Saranli, Daniel E Koditschek (2003), “Back Flips with a Hexapedal Robot”, Reprinted from Proceedings of the IEEE International Conference on Robotics and Automation, Volume 3, 2003, pages 22092215 12 Sarjoun Skaff, Alfred A Rizzi, Howie Choset, Pei-Chun Lin (2005), “A Context-Based State Estimation Technique for Hybrid Systems”, Proceedings of the 2005 IEEE international Conference on Robottics and Automation Barcelona, Spain 13 Tran, D T., Luu, M H., Nguyen, T L., Nguyen, D D., & Nguyen, P T (2007) Land-Vehicle MEMS INS/GPS Positioning During GPS Signal Blockage Periods Journal of Science, Vietnam National University, Hanoi, 23(4), 243-251 14 Tran, D T., Luu, M H., Nguyen, T L., Nguyen, P T., & Huynh, H T (2007) Performance Improvement of MEMS-Based Sensor Applying in Inertial Navigation Systems Posts, Telematics & Information Technology Journal, 2, 19-24 15 Duc-Tan, T., Fortier, P., & Huynh, H T (2011) Design, simulation, and performance analysis of an INS/GPS system using parallel kalman filters structure REV Journal on Electronics and Communications, 1(2) (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 54 PHỤ LỤC Mã nguồn chương trình include "PinChangeInt.h" #include // Thanks to Brett Beauregard for his nice PID library #define encodPinA1 // Quadrature encoder A pin #define encodPinB1 // Quadrature encoder B pin #define M1 // PWM outputs to L298N H-Bridge motor driver module #define M2 10 double kp = , ki = , kd = 0.01 ,input = 0, output = 0, setpoint = 0; // modify kp, ki and kd for optimal performance long temp; volatile long encoderPos = 0; PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT' void setup() { pinMode(encodPinA1, INPUT_PULLUP); // quadrature encoder input A pinMode(encodPinB1, INPUT_PULLUP); // quadrature encoder input B attachInterrupt(0, encoder, FALLING); // update encoder position (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 55 TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise myPID.SetMode(AUTOMATIC); myPID.SetSampleTime(1); myPID.SetOutputLimits(-255, 255); } void loop() { temp += analogRead(0); // increment position target with potentiometer value (speed), potmeter connected to A0 if (temp < 0) { // in case of overflow encoderPos = 0; temp = 0; } setpoint = temp / 500; // modify division to fit motor and encoder characteristics input = encoderPos ; // data from encoder myPID.Compute(); // calculate new output pwmOut(output); // drive L298N H-Bridge module } void pwmOut(int out) { // to H-Bridge board (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 56 if (out > 0) { analogWrite(M1, out); // drive motor CW analogWrite(M2, 0); } else { analogWrite(M1, 0); analogWrite(M2, abs(out)); // drive motor CCW } } void encoder() { // pulse and direction, direct port reading to save cycles if (PINB & 0b00000001) encoderPos++; // (digitalRead(encodPinB1)==HIGH) count ++; else encoderPos ; // if (digitalRead(encodPinB1)==LOW) count ; } POSITION CONTROL // PID motor position control // Thanks to Brett Beauregard for his nice PID library #include #include (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com if (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 57 #define encodPinA1 // Quadrature encoder A pin #define encodPinB1 // Quadrature encoder B pin #define M1 // PWM outputs to L298N H-Bridge motor driver module #define M2 10 double kp = , ki = , kd = 0.01; // modify for optimal performance double input = 0, output = 0, setpoint = 0; long temp; volatile long encoderPos = 0; PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT); // if motor will only run at full speed try 'REVERSE' instead of 'DIRECT' void setup() { pinMode(encodPinA1, INPUT_PULLUP); // quadrature encoder input A pinMode(encodPinB1, INPUT_PULLUP); // quadrature encoder input B attachInterrupt(0, encoder, FALLING); // update encoder position (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 58 TCCR1B = TCCR1B & 0b11111000 | 1; // set 31KHz PWM to prevent motor noise myPID.SetMode(AUTOMATIC); myPID.SetSampleTime(1); myPID.SetOutputLimits(-255, 255); Serial.begin (115200); // for debugging } void loop() { setpoint = analogRead(0) / 5; // modify to fit motor and encoder characteristics, potmeter connected to A0 input = encoderPos ; // Serial.println(encoderPos); myPID.Compute(); pwmOut(output); // data from encoder // monitor motor position // calculate new output // drive L298N H-Bridge module } void pwmOut(int out) { // to H-Bridge board if (out > 0) { (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 TIEU LUAN MOI download : skknchat@gmail.com (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103 (LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103(LUAN.van.THAC.si).nghien.cuu thiet.ke.mach.dieu.khien.dong.co.ung.dung.cho.robot.rhex.luan.van.ths.ky.thuat.dien dien.tu.va.vien.thong.85103