Đang tải... (xem toàn văn)
Đồ án tốt nghiệp loại xuất sắc của sinh viên cơ điện tử trường đại học bách khoa hà nội năm 2021 Đồ án tốt nghiệp của nhóm em thực hiện thiết kế và chế tạo robot 6 bậc tự do điều khiển qua App. Trong phạm vi đồ án, nhóm đặt ra mục tiêu là tính toán chính xác các phần động học, động lực học, mô hình hóa robot trên Matlab Simulink, thiết kế giao diện điều khiển và để robot chạy đến các tọa độ mong muốn
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ BỘ MÔN CƠ ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP Thiết kế điều khiển cánh tay Robot bậc NGUYỄN NĂNG NAM HẢI hai.nnn161301@sis.hust.edu.vn VŨ VĂN HIỆP hiep.vv161459@sis.hust.edu.vn Chuyên ngành Cơ Điện Tử Giảng viên hướng dẫn: TS Phạm Đức An Chữ ký GVHD Bộ môn: Cơ điện tử Viện: Cơ khí HÀ NỘI, 6/2021 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI CỘNG HỒ XÃ HỘI CHỦ NGHÍA VIỆT NAM Độc lập – Tự – Hạnh phúc NHIỆM VỤ THIẾT KẾ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Nguyễn Năng Nam Hải Vũ Văn Hiệp MSSV: 20161301 MSSV: 20161459 Lớp: KT Cơ điện tử – K61 Bộ môn: Cơ Điện Tử Viện: Cơ Khí I/ ĐỀ TÀI THIẾT KẾ “Thiết kế điều khiển cánh tay Robot bậc tự do” II/ CÁC SỐ LIỆU BAN ĐẦU - Cánh tay Robot bậc III/ NỘI DUNG THUYẾT MINH VÀ TÍNH TOÁN - Tổng quan - Thiết kế sơ đồ nguyên lý tính tốn kết cấu phần khí - Xây dựng sơ đồ điều khiển lựa chọn mạch điều khiển IV/ CÁC BẢN VẼ VÀ ĐỒ THỊ - Bản vẽ lắp (A0) - Bản vẽ kết cấu vẽ mạch điện (A0) - Bản vẽ sơ đồ điều khiển (A0) V/ CÁN BỘ HƯỚNG DẪN: TS Phạm Đức An VI/ NGÀY GIAO NHIỆM VỤ THIẾT KẾ: 15/03/2021 VII/ NGÀY HOÀN THÀNH ĐỒ ÁN: 28/06/2021 Hà Nội, ngày….tháng… năm 2021 Giảng viên hướng dẫn Đánh giá giảng viên hướng dẫn …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… Kết đánh giá Họ tên Nguyễn Năng Nam Hải Vũ Văn Hiệp Điểm Hà Nội, ngày….tháng… năm 2021 Giảng viên hướng dẫn Đánh giá giảng viên phản biện …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… Kết đánh giá Họ tên Nguyễn Năng Nam Hải Vũ Văn Hiệp Điểm Hà Nội, ngày… tháng… năm 2021 Giảng viên phản biện Lời cảm ơn Trải qua năm học tập trường Đại học Bách Khoa Hà Nội, nơi trang bị cho chúng em kiến thức chuyên môn đời sống để có hành trang tốt cho tương lai Đồ án tốt nghiệp ngày hôm không phần chương trình học tập mà hết điều kiện để chúng em áp dụng kiến thức học tự đánh giá lại kiến thức thân để tạo tiền đề cho cơng việc tương lai Chính kiến thức tổng hợp đầy đủ tích lũy chúng em Qua đây, nhóm xin gửi lời cảm ơn chân thành tới thầy cô viện Cơ khí, mơn Cơ điện tử đặc biệt thầy giáo TS.Phạm Đức An, người sát cánh giúp đỡ chúng em kì học vừa qua.Chúng em xin chúc thầy cô mạnh khỏe, nhiệt huyết để dạy bảo, giúp đỡ hệ sinh viên Cảm ơn gia đình ln tin tưởng hi vọng, nguồn động lực lớn cho chúng (3*dq(1)*cos(q(3))*cos(q(4))^2*cos(q(5))^2*sin(q(3)))/50000 + (3*dq(4)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 (3*dq(4)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 (3*dq(4)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 (3*dq(1)*cos(q(2))*cos(q(3))^2*cos(q(4))^2*cos(q(5))^2*sin(q(2)))/2500 (3*dq(1)*cos(q(2))^2*cos(q(3))*cos(q(4))^2*cos(q(5))^2*sin(q(3)))/2500 + (3*dq(1)*cos(q(2))^2*cos(q(3))^2*cos(q(4))*cos(q(5))*sin(q(5)))/12500 + (3*dq(5)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(4))*sin(q(5)))/ 50000 (3*dq(5)*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(4))*sin(q(5)))/ 50000 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*s in(q(5)))/12500; psi32=(3*dq(5)*cos(q(5))*sin(q(4))^2*sin(q(5)))/50000 dq(4)*((cos(q(4))*sin(q(4)))/10000 (3*cos(q(4))*sin(q(4))*sin(q(5))^2)/50000); psi33=(3*dq(5)*cos(q(5))*sin(q(4))^2*sin(q(5)))/50000 dq(4)*((cos(q(4))*sin(q(4)))/10000 (3*cos(q(4))*sin(q(4))*sin(q(5))^2)/50000); psi34=(39*dq(5)*sin(q(4)))/200000 - (dq(3)*sin(2*q(4)))/50000 (dq(2)*sin(2*q(4)))/50000 - (9*dq(1)*cos(q(2))*cos(q(3)))/100000 + (9*dq(1)*sin(q(2))*sin(q(3)))/100000 (3*dq(5)*cos(q(5))^2*sin(q(4)))/50000 (3*dq(4)*cos(q(4))*cos(q(5))*sin(q(5)))/50000 (dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2)/25000 (3*dq(2)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 (3*dq(3)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 + (dq(1)*cos(q(4))^2*sin(q(2))*sin(q(3)))/25000 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2*cos(q(5))^2)/50000 + (3*dq(1)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 (3*dq(1)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 (3*dq(1)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000; psi35=(3*dq(2)*sin(2*q(5)))/100000 + (3*dq(3)*sin(2*q(5)))/100000 + (39*dq(4)*sin(q(4)))/200000 - (3*dq(4)*cos(q(5))^2*sin(q(4)))/50000 + (39*dq(1)*cos(q(2))*sin(q(3))*sin(q(4)))/200000 + (39*dq(1)*cos(q(3))*sin(q(2))*sin(q(4)))/200000 (3*dq(2)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 (3*dq(3)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 (3*dq(1)*cos(q(2))*cos(q(5))^2*sin(q(3))*sin(q(4)))/50000 (3*dq(1)*cos(q(3))*cos(q(5))^2*sin(q(2))*sin(q(4)))/50000 + (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(4))*sin(q(5)))/ 50000 (3*dq(1)*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(4))*sin(q(5)))/ 50000; psi41=(9*dq(2)*cos(q(2))*cos(q(3)))/100000 - (dq(1)*sin(2*q(4)))/50000 + (9*dq(3)*cos(q(2))*cos(q(3)))/100000 (9*dq(2)*sin(q(2))*sin(q(3)))/100000 (9*dq(3)*sin(q(2))*sin(q(3)))/100000 (27*dq(5)*cos(q(2))*cos(q(3))*cos(q(4)))/200000 + (27*dq(5)*cos(q(4))*sin(q(2))*sin(q(3)))/200000 + (dq(2)*cos(q(2))*cos(q(3))*cos(q(4))^2)/25000 + (dq(3)*cos(q(2))*cos(q(3))*cos(q(4))^2)/25000 + (dq(1)*cos(q(2))^2*cos(q(4))*sin(q(4)))/25000 + (dq(1)*cos(q(3))^2*cos(q(4))*sin(q(4)))/25000 - 115 (3*dq(1)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 (dq(2)*cos(q(4))^2*sin(q(2))*sin(q(3)))/25000 (dq(3)*cos(q(4))^2*sin(q(2))*sin(q(3)))/25000 (3*dq(5)*cos(q(2))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 (3*dq(5)*cos(q(3))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 (3*dq(5)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))^2)/50000 + (3*dq(5)*cos(q(4))*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 + (3*dq(2)*cos(q(2))*cos(q(3))*cos(q(4))^2*cos(q(5))^2)/50000 + (3*dq(3)*cos(q(2))*cos(q(3))*cos(q(4))^2*cos(q(5))^2)/50000 (dq(1)*cos(q(2))^2*cos(q(3))^2*cos(q(4))*sin(q(4)))/12500 + (3*dq(1)*cos(q(2))^2*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 + (3*dq(1)*cos(q(3))^2*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 (3*dq(2)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 (3*dq(3)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 + (3*dq(2)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*dq(2)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 + (3*dq(3)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*dq(3)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 (3*dq(1)*cos(q(2))^2*cos(q(3))^2*cos(q(4))*cos(q(5))^2*sin(q(4)))/2500 + (3*dq(1)*cos(q(2))*cos(q(5))*sin(q(2))*sin(q(4))*sin(q(5)))/50000 + (3*dq(1)*cos(q(3))*cos(q(5))*sin(q(3))*sin(q(4))*sin(q(5)))/50000 (3*dq(1)*cos(q(2))*cos(q(3))^2*cos(q(5))*sin(q(2))*sin(q(4))*sin(q(5)) )/25000 (3*dq(1)*cos(q(2))^2*cos(q(3))*cos(q(5))*sin(q(3))*sin(q(4))*sin(q(5)) )/25000 + (dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*sin(q(2))*sin(q(3))*sin(q(4)))/12 500 + (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))^2*sin(q(2))*sin(q(3)) *sin(q(4)))/25000; psi42=(dq(2)*sin(2*q(4)))/50000 + (dq(3)*sin(2*q(4)))/50000 (27*dq(5)*sin(q(4)))/200000 + (9*dq(1)*cos(q(2))*cos(q(3)))/100000 (9*dq(1)*sin(q(2))*sin(q(3)))/100000 (3*dq(5)*cos(q(5))^2*sin(q(4)))/50000 + (dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2)/25000 + (3*dq(2)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 + (3*dq(3)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 (dq(1)*cos(q(4))^2*sin(q(2))*sin(q(3)))/25000 + (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2*cos(q(5))^2)/50000 (3*dq(1)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 + (3*dq(1)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*dq(1)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000; psi43=(dq(2)*sin(2*q(4)))/50000 + (dq(3)*sin(2*q(4)))/50000 (27*dq(5)*sin(q(4)))/200000 + (9*dq(1)*cos(q(2))*cos(q(3)))/100000 (9*dq(1)*sin(q(2))*sin(q(3)))/100000 (3*dq(5)*cos(q(5))^2*sin(q(4)))/50000 + (dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2)/25000 + (3*dq(2)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 + (3*dq(3)*cos(q(4))*cos(q(5))^2*sin(q(4)))/50000 (dq(1)*cos(q(4))^2*sin(q(2))*sin(q(3)))/25000 + (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))^2*cos(q(5))^2)/50000 (3*dq(1)*cos(q(4))^2*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 + (3*dq(1)*cos(q(2))*cos(q(4))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*dq(1)*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(5)))/50000; psi44=-(3*dq(5)*sin(2*q(5)))/100000; psi45=(27*dq(1)*cos(q(4))*sin(q(2))*sin(q(3)))/200000 (27*dq(2)*sin(q(4)))/200000 - (27*dq(3)*sin(q(4)))/200000 (3*dq(2)*cos(q(5))^2*sin(q(4)))/50000 - 116 (3*dq(3)*cos(q(5))^2*sin(q(4)))/50000 (27*dq(1)*cos(q(2))*cos(q(3))*cos(q(4)))/200000 (3*dq(4)*sin(2*q(5)))/100000 (3*dq(1)*cos(q(2))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 (3*dq(1)*cos(q(3))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))^2)/50000 + (3*dq(1)*cos(q(4))*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000; psi51=dq(4)*((27*cos(q(2))*cos(q(3))*cos(q(4)))/200000 (27*cos(q(4))*sin(q(2))*sin(q(3)))/200000 + (3*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))^2)/50000 (3*cos(q(4))*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000 + (3*cos(q(2))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*cos(q(3))*cos(q(5))*sin(q(2))*sin(q(5)))/50000) dq(1)*((3*cos(q(2) + q(3))^2*cos(q(5))*sin(q(5)))/50000 (3*cos(q(5))*sin(q(5)))/50000 + (3*cos(q(2) + q(3))*sin(q(2) + q(3))*cos(q(4)))/50000 + (3*cos(q(2) + q(3))^2*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 - (3*cos(q(2) + q(3))*sin(q(2) + q(3))*cos(q(4))*cos(q(5))^2)/25000) (3*dq(2)*sin(q(4))*(13*cos(q(2))*sin(q(3)) + 13*cos(q(3))*sin(q(2)) 4*cos(q(2))*cos(q(5))^2*sin(q(3)) - 4*cos(q(3))*cos(q(5))^2*sin(q(2)) + 4*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(5)) 4*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(5))))/200000 (3*dq(3)*sin(q(4))*(13*cos(q(2))*sin(q(3)) + 13*cos(q(3))*sin(q(2)) 4*cos(q(2))*cos(q(5))^2*sin(q(3)) - 4*cos(q(3))*cos(q(5))^2*sin(q(2)) + 4*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(5)) 4*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(5))))/200000; psi52=(27*dq(4)*sin(q(4)))/200000 - (3*dq(3)*sin(2*q(5)))/100000 (3*dq(2)*sin(2*q(5)))/100000 + (3*dq(4)*cos(q(5))^2*sin(q(4)))/50000 (39*dq(1)*cos(q(2))*sin(q(3))*sin(q(4)))/200000 (39*dq(1)*cos(q(3))*sin(q(2))*sin(q(4)))/200000 + (3*dq(2)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 + (3*dq(3)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 + (3*dq(1)*cos(q(2))*cos(q(5))^2*sin(q(3))*sin(q(4)))/50000 + (3*dq(1)*cos(q(3))*cos(q(5))^2*sin(q(2))*sin(q(4)))/50000 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(4))*sin(q(5)))/ 50000 + (3*dq(1)*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(4))*sin(q(5)))/ 50000; psi53=(27*dq(4)*sin(q(4)))/200000 - (3*dq(3)*sin(2*q(5)))/100000 (3*dq(2)*sin(2*q(5)))/100000 + (3*dq(4)*cos(q(5))^2*sin(q(4)))/50000 (39*dq(1)*cos(q(2))*sin(q(3))*sin(q(4)))/200000 (39*dq(1)*cos(q(3))*sin(q(2))*sin(q(4)))/200000 + (3*dq(2)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 + (3*dq(3)*cos(q(4))^2*cos(q(5))*sin(q(5)))/50000 + (3*dq(1)*cos(q(2))*cos(q(5))^2*sin(q(3))*sin(q(4)))/50000 + (3*dq(1)*cos(q(3))*cos(q(5))^2*sin(q(2))*sin(q(4)))/50000 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))*sin(q(4))*sin(q(5)))/ 50000 + (3*dq(1)*cos(q(4))*cos(q(5))*sin(q(2))*sin(q(3))*sin(q(4))*sin(q(5)))/ 50000; psi54=(3*dq(4)*sin(2*q(5)))/100000 + (27*dq(2)*sin(q(4)))/200000 + (27*dq(3)*sin(q(4)))/200000 + (3*dq(2)*cos(q(5))^2*sin(q(4)))/50000 + (3*dq(3)*cos(q(5))^2*sin(q(4)))/50000 + (27*dq(1)*cos(q(2))*cos(q(3))*cos(q(4)))/200000 (27*dq(1)*cos(q(4))*sin(q(2))*sin(q(3)))/200000 + (3*dq(1)*cos(q(2))*cos(q(5))*sin(q(3))*sin(q(5)))/50000 + (3*dq(1)*cos(q(3))*cos(q(5))*sin(q(2))*sin(q(5)))/50000 + 117 (3*dq(1)*cos(q(2))*cos(q(3))*cos(q(4))*cos(q(5))^2)/50000 (3*dq(1)*cos(q(4))*cos(q(5))^2*sin(q(2))*sin(q(3)))/50000; psi55=0; Psi=[psi11, psi12, psi13, psi14, psi15;psi21, psi22, psi23, psi24, psi25; psi31, psi32, psi33, psi34, psi35; psi41, psi42, psi43, psi44, psi45; psi51, psi52, psi53, psi54, psi55;]; % Luc suy rong cua cac luc co the g1 = 0; g2 = (981*h34*(cos(q(2))*cos(q(3)) - sin(q(2))*sin(q(3))))/500 + (36297*(h3 + h4/2)*(cos(q(2))*cos(q(3)) - sin(q(2))*sin(q(3))))/10000 + (6867*h2*cos(q(2)))/400 + (981*h5*sin(q(5))*(cos(q(2))*cos(q(3)) sin(q(2))*sin(q(3))))/1000 (981*h5*cos(q(4))*cos(q(5))*(cos(q(2))*sin(q(3)) + cos(q(3))*sin(q(2))))/1000;%m2*g/2*a2*cos(q(1)+q(2)); g3=(981*h34*(cos(q(2))*cos(q(3)) - sin(q(2))*sin(q(3))))/500 + (36297*(h3 + h4/2)*(cos(q(2))*cos(q(3)) - sin(q(2))*sin(q(3))))/10000 + (981*h5*sin(q(5))*(cos(q(2))*cos(q(3)) - sin(q(2))*sin(q(3))))/1000 - (981*h5*cos(q(4))*cos(q(5))*(cos(q(2))*sin(q(3)) + cos(q(3))*sin(q(2))))/1000; g4=-(981*h5*cos(q(5))*sin(q(4))*(cos(q(2))*cos(q(3)) sin(q(2))*sin(q(3))))/1000; g5=(981*h5*cos(q(5))*(cos(q(2))*sin(q(3)) + cos(q(3))*sin(q(2))))/1000 - (981*h5*cos(q(4))*sin(q(5))*(cos(q(2))*cos(q(3)) sin(q(2))*sin(q(3))))/1000; G = [g1; g2; g3; g4; g5]; Q1=0.2; Q2=0.2; Q3=0.2; Q4=0.2; Q5=0.2; Q=[Q1;Q2;Q3;Q4;Q5]; % Gia toc suy rong ddq = inv(M+r*r*Jm)*(Km*R_inv*[U(1);U(2);U(3);U(4);U(5)](Psi+Km*R_inv*Ke*r*r)*[dq(1);dq(2);dq(3);dq(4);dq(5)]-G-Q); 118 PHỤ LỤC 3: CODE GIAO TIẾP MÁY TÍNH VỚI ARDUINO #include //#include #define PI 3.1415926535897932384626433832795 //driver for the axis #define PUL1_PIN A0 #define DIR1_PIN A1 //driver for the axis #define PUL2_PIN A6 #define DIR2_PIN A7 //driver for the axis #define PUL3_PIN 46 #define DIR3_PIN 48 //driver for the axis #define PUL4_PIN 26 #define DIR4_PIN 28 //driver for the axis #define PUL5_PIN 36 #define DIR5_PIN 34 //enable pin for the axis #define EN1_PIN 38 #define EN2_PIN A2 #define EN3_PIN A8 #define EN4_PIN 24 #define EN5_PIN 30 double curPos1 = 0.0; double curPos2 = -0.0; double curPos3 = 0.0; double curPos4 = 0.0; double curPos5 = -0.0; boolean PULstat1 = 0; boolean PULstat2 = 0; boolean PULstat3 = 0; boolean PULstat4 = 0; boolean PULstat5 = 0; //robot motor const double dl1 = 360.0/200.0/32.0/7; const double dl2 = 360.0/200.0/32.0/23.0; const double dl3 = 360.0/200.0/32.0/5.0; const double dl4 = 360.0/200.0/32.0/1.0; 119 const double dl5 = 360.0/200.0/32.0/3.2; //SoftwareSerial Bluetooth(29, Bluetooth (TX, RX) 31); // Arduino(RX, TX) - HC-05 String dataIn = ""; //variable to store the bluetooth command double futPos1 = 0.0; double futPos2 = 0.0; double futPos3 = 0.0; double futPos4 = 0.0; double futPos5 = 0.0; double curSpeed = 0.3*0.5e-4; double curFinalSpeed = 0.0; int index = 0; //index corresonding to the robot position float Joint1[50], Joint2[50], Joint3[50], MaxSpeed[50], InSpeed[50], FinSpeed[50]; Joint4[50], Joint5[50], void setup() { //Bluetooth.begin(38400); module // Default baud rate of the Bluetooth //Bluetooth.setTimeout(1); Serial1.begin(9600); //Serial1.setTimeout(3); delay(20); pinMode(PUL1_PIN, OUTPUT); pinMode(DIR1_PIN, OUTPUT); pinMode(PUL2_PIN, OUTPUT); pinMode(DIR2_PIN, OUTPUT); pinMode(PUL3_PIN, OUTPUT); pinMode(DIR3_PIN, OUTPUT); pinMode(PUL4_PIN, OUTPUT); pinMode(DIR4_PIN, OUTPUT); pinMode(PUL5_PIN, OUTPUT); pinMode(DIR5_PIN, OUTPUT); pinMode(EN3_PIN, OUTPUT); pinMode(EN2_PIN, OUTPUT); pinMode(EN1_PIN, OUTPUT); pinMode(EN4_PIN, OUTPUT); pinMode(EN5_PIN, OUTPUT); digitalWrite(PUL1_PIN, LOW); digitalWrite(DIR1_PIN, LOW); //LOW = negative direction digitalWrite(PUL2_PIN, LOW); digitalWrite(DIR2_PIN, LOW); //LOW = positive direction 120 digitalWrite(PUL3_PIN, LOW); digitalWrite(DIR3_PIN, LOW); //LOW = negative direction digitalWrite(PUL4_PIN, LOW); digitalWrite(DIR4_PIN, LOW); //LOW = positive direction digitalWrite(PUL5_PIN, LOW); digitalWrite(DIR5_PIN, LOW); //LOW = positive direction // all joints disabled! digitalWrite(EN1_PIN, HIGH); digitalWrite(EN2_PIN, HIGH); digitalWrite(EN3_PIN, HIGH); digitalWrite(EN4_PIN, HIGH); digitalWrite(EN5_PIN, HIGH); //Serial.begin(9600); } void loop() { if (Serial1.available() > 0) { dataIn = Serial1.readString(); // Read the data as string //Serial.println(dataIn); if (dataIn == "enable") { digitalWrite(EN1_PIN, LOW); digitalWrite(EN2_PIN, LOW); digitalWrite(EN3_PIN, LOW); digitalWrite(EN4_PIN, LOW); digitalWrite(EN5_PIN, LOW); } if (dataIn == "disable") { digitalWrite(EN1_PIN, HIGH); digitalWrite(EN2_PIN, HIGH); digitalWrite(EN3_PIN, HIGH); digitalWrite(EN4_PIN, HIGH); digitalWrite(EN5_PIN, HIGH); } if (dataIn.startsWith("s5")) { String dataInS = dataIn.substring(2, dataIn.length()); futPos5 = dataInS.toFloat(); float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={curPos1, curPos2, curPos3, curPos4, futPos5}; 121 goStrightLine(Jinitial, Jfinal, curSpeed, 0.75e-10, 0.0, 0.0); } if (dataIn.startsWith("s4")) { String dataInS = dataIn.substring(2, dataIn.length()); futPos4 = dataInS.toFloat(); float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={curPos1, curPos2, curPos3, futPos4, curPos5}; goStrightLine(Jinitial, Jfinal, curSpeed, 0.75e-10, 0.0, 0.0); } if (dataIn.startsWith("s3")) { String dataInS = dataIn.substring(2, dataIn.length()); futPos3 = dataInS.toFloat(); float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={curPos1, curPos2, futPos3, curPos4, curPos5}; goStrightLine(Jinitial, Jfinal, curSpeed, 0.75e-10, 0.0, 0.0); } if (dataIn.startsWith("s2")) { String dataInS = dataIn.substring(2, dataIn.length()); futPos2 = dataInS.toFloat(); float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={curPos1, futPos2, curPos3, curPos4, curPos5}; goStrightLine(Jinitial, Jfinal, curSpeed, 0.75e-10, 0.0, 0.0); } if (dataIn.startsWith("s1")) { String dataInS = dataIn.substring(2, dataIn.length()); futPos1 = dataInS.toFloat(); float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={futPos1, curPos2, curPos3, curPos4, curPos5}; goStrightLine(Jinitial, Jfinal, curSpeed, 0.75e-10, 0.0, 0.0); } // set the maximum speed for the move if (dataIn.startsWith("ss")) { String dataInS = dataIn.substring(2, dataIn.length()); curSpeed = (dataInS.toFloat()/100)*0.5e-4; } // If button "SAVE" is pressed if (dataIn.startsWith("save")) { String dataInS = dataIn.substring(4, dataIn.length()); InSpeed[index] = curFinalSpeed; curFinalSpeed = (dataInS.toFloat()/100)*0.5e-4; Joint1[index] = curPos1; // save position into the array 122 Joint2[index] = curPos2; Joint3[index] = curPos3; Joint4[index] = curPos4; Joint5[index] = curPos5; MaxSpeed[index] = curSpeed; FinSpeed[index] = curFinalSpeed; index++; // Increase the array index } if ( dataIn == "reset") { index = 0; // Index to } if (dataIn.startsWith("run")) { float Jinitial[5]={curPos1, curPos2, curPos3, curPos4, curPos5}; float Jfinal[5]={Joint1[0], Joint2[0], Joint3[0], Joint4[0], Joint5[0]}; goStrightLine(Jinitial, Jfinal, MaxSpeed[0], 0.75e-10, 0.0, 0.0); for (int i = 0; i dl1/2.0) { if (PULstat1 == 0) { digitalWrite(PUL1_PIN, HIGH); PULstat1 = 1; } else { digitalWrite(PUL1_PIN, LOW); PULstat1 = 0; } //curPos1 = Jf[0]; curPos1 = curPos1 + dl1/2.0; 124 if (Jf[0]-curPos1>dl1/2.0) { delayMicroseconds(delF); } } } else { digitalWrite(DIR1_PIN, LOW); while (-Jf[0]+curPos1>dl1/2.0) { if (PULstat1 == 0) { digitalWrite(PUL1_PIN, HIGH); PULstat1 = 1; } else { digitalWrite(PUL1_PIN, LOW); PULstat1 = 0; } //curPos1 = Jf[0]; curPos1 = curPos1 - dl1/2.0; if (-Jf[0]+curPos1>dl1/2.0) { delayMicroseconds(delF); } } } // joint #2 if (Jf[1]-curPos2>0.0) { // positive direction of rotation digitalWrite(DIR2_PIN, HIGH); while (Jf[1]-curPos2>dl2/2.0) { if (PULstat2 == 0) { digitalWrite(PUL2_PIN, HIGH); PULstat2 = 1; } else { digitalWrite(PUL2_PIN, LOW); PULstat2 = 0; } //curPos2 = Jf[1]; curPos2 = curPos2 + dl2/2.0; if (Jf[1]-curPos2>dl2/2.0) { delayMicroseconds(delF); } } } else { digitalWrite(DIR2_PIN, LOW); while (-Jf[1]+curPos2>dl2/2.0) { if (PULstat2 == 0) { digitalWrite(PUL2_PIN, HIGH); PULstat2 = 1; } else { digitalWrite(PUL2_PIN, LOW); PULstat2 = 0; 125 } //curPos2 = Jf[1]; curPos2 = curPos2 - dl2/2.0; if (-Jf[1]+curPos2>dl2/2.0) { delayMicroseconds(delF); } } } // joint #3 if (Jf[2]-curPos3>0.0) { // positive direction of rotation digitalWrite(DIR3_PIN, LOW); while (Jf[2]-curPos3>dl3/2.0) { if (PULstat3 == 0) { digitalWrite(PUL3_PIN, HIGH); PULstat3 = 1; } else { digitalWrite(PUL3_PIN, LOW); PULstat3 = 0; } //curPos3 = Jf[2]; curPos3 = curPos3 + dl3/2.0; if (Jf[2]-curPos3>dl3/2.0) { delayMicroseconds(delF); } } } else { digitalWrite(DIR3_PIN, HIGH); while (-Jf[2]+curPos3>dl3/2.0) { if (PULstat3 == 0) { digitalWrite(PUL3_PIN, HIGH); PULstat3 = 1; } else { digitalWrite(PUL3_PIN, LOW); PULstat3 = 0; } //curPos3 = Jf[2]; curPos3 = curPos3 - dl3/2.0; if (-Jf[2]+curPos3>dl3/2.0) { delayMicroseconds(delF); } } } // joint #4 if (Jf[3]-curPos4>0.0) { // positive direction of rotation digitalWrite(DIR4_PIN, HIGH); while (Jf[3]-curPos4>dl4/2.0) { if (PULstat4 == 0) { 126 digitalWrite(PUL4_PIN, HIGH); PULstat4 = 1; } else { digitalWrite(PUL4_PIN, LOW); PULstat4 = 0; } //curPos4 = Jf[3]; curPos4 = curPos4 + dl4/2.0; if (Jf[3]-curPos4>dl4/2.0) { delayMicroseconds(delF); } } } else { digitalWrite(DIR4_PIN, LOW); while (-Jf[3]+curPos4>dl4/2.0) { if (PULstat4 == 0) { digitalWrite(PUL4_PIN, HIGH); PULstat4 = 1; } else { digitalWrite(PUL4_PIN, LOW); PULstat4 = 0; } //curPos4 = Jf[3]; curPos4 = curPos4 - dl4/2.0; if (-Jf[3]+curPos4>dl4/2.0) { delayMicroseconds(delF); } } } // joint #5 if (Jf[4]-curPos5>0.0) { // positive direction of rotation digitalWrite(DIR5_PIN, HIGH); while (Jf[4]-curPos5>dl5/2.0) { if (PULstat5 == 0) { digitalWrite(PUL5_PIN, HIGH); PULstat5 = 1; } else { digitalWrite(PUL5_PIN, LOW); PULstat5 = 0; } //curPos5 = Jf[4]; curPos5 = curPos5 + dl5/2.0; if (Jf[4]-curPos5>dl5/2.0) { delayMicroseconds(delF); } } } else { 127 digitalWrite(DIR5_PIN, LOW); while (-Jf[4]+curPos5>dl5/2.0) { if (PULstat5 == 0) { digitalWrite(PUL5_PIN, HIGH); PULstat5 = 1; } else { digitalWrite(PUL5_PIN, LOW); PULstat5 = 0; } //curPos5 = Jf[4]; curPos5 = curPos5 - dl5/2.0; if (-Jf[4]+curPos5>dl5/2.0) { delayMicroseconds(delF); } } } // joint #6 if (Jf[5]-curPos6>0.0) { // positive direction of rotation digitalWrite(DIR6_PIN, HIGH); while (Jf[5]-curPos6>dl6/2.0) { if (PULstat6 == 0) { digitalWrite(PUL6_PIN, HIGH); PULstat6 = 1; } else { digitalWrite(PUL6_PIN, LOW); PULstat6 = 0; } //curPos6 = Jf[5]; curPos6 = curPos6 + dl6/2.0; if (Jf[5]-curPos6>dl6/2.0) { delayMicroseconds(delF); } } } else { digitalWrite(DIR6_PIN, LOW); while (-Jf[5]+curPos6>dl6/2.0) { if (PULstat6 == 0) { digitalWrite(PUL6_PIN, HIGH); PULstat6 = 1; } else { digitalWrite(PUL6_PIN, LOW); PULstat6 = 0; } //curPos6 = Jf[5]; curPos6 = curPos6 - dl6/2.0; if (-Jf[5]+curPos6>dl6/2.0) { delayMicroseconds(delF); 128 } } } } 129 ... B (mm) Vmax ( v/p) 60 8-2Z 800,008 2200,009 00,12 30000 62 5 500,0 76 160 ,0 76 50,012 0,012 2300 62 4 400,0 76 130,0 76 50,012 0,012 3100 62 3-2Z 300,005 1000,005 400,025 560 00 33 ... 63 Thuật toán điều khiển 63 Sơ đồ kết nối thiết bị điều khiển: 66 CHƯƠNG MƠ HÌNH THỰC NGHIỆM VÀ KẾT QUẢ 67 4.1 Mơ hình thực nghiệm 67 4.2 Kết 69 Độ... thống điều khiển 63 Hình 3.45: Sơ đồ thuật toán hàm khối thực thi 64 Hình 3. 46: Sơ đồ kết nối thành phần điều khiển 66 Hình 4.1: Một số góc nhìn Robot 67 Hình 4.2: Một số