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 Tieu luan Tieu luan 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 TỐN - Tổng quan - Thiết kế sơ đồ ngun 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 Tieu luan Tieu luan Đánh giá giảng viên hướng dẫn …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… Kết đánh giá Họ tên Điểm Hà Nội, ngày….tháng… năm 2021 Giảng viên hướng dẫn Nguyễn Năng Nam Hải Vũ Văn Hiệp Tieu luan Tieu luan Đánh giá giảng viên phản biện …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… Kết đánh giá Họ tên Điểm Hà Nội, ngày… tháng… năm 2021 Giảng viên phản biện Nguyễn Năng Nam Hải Vũ Văn Hiệp Tieu luan Tieu luan 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 chun 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 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 Tieu luan Tieu luan (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 - Tieu luan 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 - Tieu luan 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 + Tieu luan 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); Tieu luan 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; Tieu luan 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 Tieu luan 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}; Tieu luan 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 Tieu luan 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; Tieu luan 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; Tieu luan 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) { Tieu luan 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 { Tieu luan 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); Tieu luan 128 } } } } Tieu luan 129 ... mềm điều khiển 57 3.4 Thuật toán điều khiển sơ đồ kết nối 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 ... 20 161 301 MSSV: 20 161 459 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. .. ứng dụng sản xuất - - Robot thiết kế khung cánh tay bậc tự có đặc điểm sau: Khung cánh tay máy bao gồm chân đế lắp trục quay cánh tay máy, trục quay có khớp quay Cuối cánh tay máy mặt bích xốy