Đồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minh

Đồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minhĐồ án tốt nghiệp: Thiết kế và điều khiển Robot 6 bậc tự do không ràng buộc sử dụng kỹ thuật động học nghịch thông minh

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BẬC TỰ DO KHÔNG RÀNG BUỘC SỬ DỤNG KỸ THUẬT ĐỘNG HỌC NGHỊCH THƠNG MINH SVTH : ĐẶNG SỸ BÌNH MSSV : 16151002 KHÓA : 2016 - 2020 NGÀNH : CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD : TS ĐẶNG XUÂN BA Hồ Chí Minh, tháng năm 2020 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BẬC TỰ DO KHÔNG RÀNG BUỘC SỬ DỤNG KỸ THUẬT ĐỘNG HỌC NGHỊCH THƠNG MINH SVTH : ĐẶNG SỸ BÌNH MSSV : 16151002 KHÓA : 2016 - 2020 NGÀNH : CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD : TS ĐẶNG XUÂN BA Hồ Chí Minh, tháng năm 2020 LỜI CẢM ƠN Lời đầu tiên nhóm chúng em xin gửi lời tri ân biết ơn sâu sắc đến Thầy – Tiến sĩ Đặng Xuân Ba, người trực tiếp hướng dẫn đồ án tốt nghiệp, tận tình bảo, động viên, khích lệ chúng em suốt q trình nghiên cứu, thực đề tài Em xin gửi lời cảm ơn đến thầy cô Khoa Đào tạo Chất lượng cao, Trường Đại học Sư phạm – Kỹ thuật nhiệt tình giảng dạy cho chúng em kiến thức môn đại cương môn chuyên ngành, giúp em có sở lý thuyết vững vàng tạo điều kiện giúp đỡ chúng em suốt bốn năm học tập Xin cảm ơn gia đình ln bên động viên cho em Bên cạnh đó, em xin chân thành cảm ơn người bạn Lê Cơng Long, người góp cơng sức nhiều đồ án em với lời cảm ơn tới tồn tập thể thành viên phịng thí nghiệm Dynamics Robotics and Control Nếu khơng có sự giúp sức tất người, em hồn thành đồ án mong đợi Cuối chúng em xin kính chúc thầy dồi sức khỏe gặt hái nhiều thành công sự nghiệp giảng dạy cao quý Đồng kính chúc người thân bạn bè có nhiều sức khỏe thành cơng sống TP Hồ Chí Minh, tháng Năm 2019 Sinh viên thực Đặng Sỹ Bình iv TĨM TẮT ĐỒ ÁN Dựa vào kiến thức học điều khiển tự động sự tìm hiểu ngành công nghiệp robot suốt năm đại học, chúng em nhận thấy nghiên cứu robot lĩnh vực quan tâm ngày Cho đến nay, loại robot tự động, bán tự động hay cao cấp robot với trí tuệ nhân tạo ngày phổ biến phát triển rộng rãi nơi Nắm bắt xu hướng nghiên cứu ngành điều khiển tự động, chúng em định chọn đề tài đồ án tốt nghiệp liên quan đến robot Tuy nhiên, kiến thức chuyên ngành hạn chế mà lĩnh vực robot phức tạp tân tiến, nên chúng em cho thật thú vị chúng em thiết kế, chế tạo điều khiển robot làm việc cách tồn diện khơng gian Với đề tài đồ án tốt nghiệp “THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BẬC TỰ DO KHÔNG RÀNG BUỘC SỬ DỤNG KỸ THUẬT ĐỘNG HỌC NGHỊCH THÔNG MINH”, chúng em thực nội dung sau:     Giải toán động học thuận động học nghịch Mô robot phần mềm Matlab Thiết kế chế tạo mơ hình thực nghiệm robot bậc tự Tiến hành điều khiển robot thực tế với kỹ thuật điều khiển thơng minh Nhóm chúng em tiến hành mơ phương trình động học, đồng thời nghiên cứu chế tạo mơ hình Sau mơ hình nhiệm vụ mơ hồn thành, chúng em tiến hành áp dụng tính tốn nghiên cứu lên mơ hình thực v ABSTRACT Based on the knowledge learned about automatic control and our understanding of the robotics industry today during the university years, we have found robot research to be one of the most relevant fields today Until now, types of robots that are automatic, semi-automatic, or more advanced than robots with artificial intelligence are increasingly popular and widely developed everywhere Understanding the research trend of the automation industry, we decided to choose a graduate project related to robots However, due to limited specialized knowledge and the field of robotics is very complex and advanced, we think it will be interesting if we can design, manufacture, and control a robot Comprehensively working in space With the theme of graduation project "DESIGN AND CONTROL DOF ROBOT WITH INTELLECTUAL DYNAMIC ENGINEERING TECHNOLOGY", we have implemented the following contents:     Solving forward and inverse kinematic problems Simulating robots on Matlab software Design and manufacture of a experimental model of the DOF robot chosen Conduct realistic robot control with intelligent control techniques I conduct kinetic equation simulations, and researches and creates models After the model and simulation task completed, we proceed to apply the researched calculations to the real model vi MỤC LỤC NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP i PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN ii PHIẾU NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN iii LỜI CẢM ƠN iv TÓM TẮT ĐỒ ÁN v ABSTRACT vii MỤC LỤC viiii DANH MỤC CÁC TỪ VIẾT TẮT xiii DANH MỤC CÁC BẢNG BIỂU xiv DANH MỤC CÁC HÌNH ẢNH xv CHƯƠNG TỔNG QUAN ĐỀ TÀI 1.1 Tính cấp thiết đề tài 1.2 Tổng quan robot UR3 robot 1.2.1 Sơ lược lịch sử robot 1.2.2 Tổng quan Robot DRC3 10 1.3 Mục tiêu nghiên cứu 10 1.4 Đối tượng nghiên cứu, phạm vi nghiên cứu giới hạn đề tài 11 1.4.1 Đối tượng nghiên cứu 11 1.4.2 Phạm vi nghiên cứu 11 1.4.3 Giới hạn đề tài 11 1.5 Ý nghĩa khoa học thực tiễn đề tài 11 1.5.1 Ý nghĩa khoa học đề tài 11 1.5.2 Ý nghĩa thực tiễn đề tài 11 1.6 Phương pháp nghiên cứu 11 1.7 Kết cấu luận văn 12 CHƯƠNG CƠ SỞ LÝ THUYẾT 13 vii 2.1 Nguyên lí hoạt động 13 2.2 Phần mơ hình 13 2.2.1 Cơ sở ý tưởng mơ hình 14 2.2.2 Thiết kế mơ hình dùng SOLIDWORKS 14 2.3 2.2.2.1 Sơ lược SOLIDWORKS 14 2.2.2.2 Xuất file để in 3D cho chi tiết 15 Thuật toán điều khiển 17 2.3.1 Sơ lược lý thuyết điều khiển tự động 17 2.3.1.1 Các khái niệm 17 2.3.1.2 Các phần tử hệ thống ĐKTĐ 17 2.3.1.3 Phân loại hệ thống ĐKTĐ 18 2.3.1.4 Các nguyên tắc điều khiển 18 2.3.2 Phần mềm MATLAB 18 2.3.2.1 Giới thiệu MATLAB 18 2.3.2.2 Tổng quan cấu trúc liệu ứng dụng MATLAB 19 2.3.2.3 Hệ thống MATLAB 19 2.3.3 Quy hoạch quỹ đạo cho Robot 20 CHƯƠNG TÍNH TỐN, MƠ PHỎNG VÀ KIỂM CHỨNG ĐỘNG HỌC CỦA ROBOT BẰNG MATLAB 23 3.1 Tính tốn, mơ kiểm chứng động học thuận 23 3.1.1 Động học thuận 23 3.1.2 Mơ phỏng, kiểm chứng kết tính tốn 25 3.2 Tính tốn kiểm chứng động học nghịch 25 3.2.1 Tính toán động học nghịch phương pháp đại số 25 3.2.2 Mô Phỏng, kiểm chứng kết tính toán phương pháp đại số 26 3.2.3 Tính toán động học nghịch phương pháp thông minh 27 3.2.4 Mô phỏng, kiểm chứng kết tính tốn phương pháp thơng minh 29 3.2.4.1 So sánh thử nghiệm giải pháp tĩnh 30 3.2.4.2 Thử nghiệm cho Robot vẽ theo quỹ đạo 32 viii 3.3 Nhận xét 36 3.4 Kết luận 36 CHƯƠNG THIẾT KẾ VÀ THI CƠNG MƠ HÌNH DRC3 ROBOT 37 4.1 Lựa chọn mơ hình robot UR3 37 4.2 Chế tạo mơ hình cho robot DRC3 38 4.2.1 Khớp thứ 38 4.2.2 Khớp thứ 39 4.2.3 Khớp thứ 39 4.2.4 Khớp thứ 41 4.2.5 Khớp thứ 42 4.2.6 Khớp thứ 43 4.2.7 Driver MR-J2S-10A 43 4.2.8 Động HC-KFS053 47 4.2.9 Động HC-KFS13 49 4.3 Thiết kế phần mềm 50 4.3.1 Giới thiệu ARDUINO 50 4.3.2 Giới thiệu phần mềm C# 52 4.3.3 Các thiết bị hỗ trợ xử lý : Opto quang 55 CHƯƠNG ĐIỀU KHIỂN ROBOT DRC3 - 6DOF 57 5.1 Thiết bị sử dụng 57 5.2 Kết nối phần cứng 59 5.3 Giao diện điều khiển 59 CHƯƠNG KẾT QUẢ VÀ HƯỚNG PHÁT TRIỂN 61 6.1 Kết đạt 61 6.2 Hướng phát triển 66 TÀI LIỆU THAM KHẢO 67 PHỤ LỤC 68 ix DANH MỤC CÁC TỪ VIẾT TẮT ĐKTĐ: Điều khiển tự động IFR: International Federation of Robotics AUV: Autonomoues Underwater Vehicles UAV: Unmanned Arial Vehicles ISO: International Standards Organization xiii DANH MỤC CÁC BẢNG BIỂU Bảng 1.1 Các Thông Số Kỹ Thuật Của Robot ERROR! BOOKMARK NOT DEFINED Bảng 3.1 Bảng D-H 24 Bảng 3.2 Các Bộ Nghiệm Có Thể Có Của Robot Bậc DRC3 26 Bảng 4.1 Thông Số Cơ Bản Của Driver MR-J2S-10A 47 Bảng 4.2 Thông Số Cơ Bản Của Động Cơ HC-KFS053 48 Bảng 4.3 Thông Số Cơ Bản Của Động Cơ HC-KFS13 50 Biểu Đồ 2.1 Sơ Đồ Nguyên Lý Hoạt Động Của Hệ Thống 13 Biểu Đồ 2.2 Sơ Đồ Tổng Quát Của Một Hệ Thống Đktđ 17 Biểu Đồ 2.3 Quỹ Đạo Bậc 22 Biểu Đồ 3.1 Hệ Trục Tọa Độ Cho Robot Bậc 23 Biểu Đồ 3.2 Quá Trình Hội Tụ Góc Của Các Khớp Với Thuật Tốn CLM 30 Biểu Đồ 3.3 Quá Trình Hội Tụ Của Sai Số Các Khớp Với Thuật Toán CLM 31 Biểu Đồ 3.4 Q Trình Hội Tụ Góc Của Các Khớp Với Thuật Toán SLM 31 Biểu Đồ 3.5 Quá Trình Hội Tụ Của Sai Số Các Khớp Với Thuật Toán SLM 32 Biểu Đồ 3.6 Biến Đổi Thơng Minh Của Các Thơng Số Học Với Thuật Tốn SLM 32 Biểu Đồ 3.7 Kết Quả Động Học Nghịch Theo Quỹ Đạo Mong Muốn 33 Biểu Đồ 3.8 Kết Quả Động Học Nghịch Theo Quỹ Đạo Mong Thực Tế 33 Biểu Đồ 3.9 Quá Trình Hội Tụ Góc Của Các Khớp Với Thuật Tốn CLM Trong Thử Nghiệm Vẽ Theo Quỹ Đạo 34 Biểu Đồ 3.10 Quá Trình Hội Tụ Của Sai Số Các Khớp Với Thuật Toán SLM Trong Thử Nghiệm Vẽ Theo Quỹ Đạo 34 Biểu Đồ 3.11 Quá Trình Hội Tụ Của Sai Số Với Thuật Toán Tự Học 35 Biểu Đồ 3.12 Biến Đổi Thông Minh Của Thông Số Học Của Thuật Toán 35 xiv t_count = 0; t_command = 0; t_quyhoach_4 = 0; error_4 = 0; //========================================================== ==== //========================================================== ==== pinMode(DIR_5, OUTPUT); // CHAN PWM pinMode(PULSE_5, OUTPUT); //chan dieu khien dong co digitalWrite(PULSE_5, LOW); //=============================== vitri_dat_5 = 0; vitri_5 = 0; vitri_thuc_5 = vitri_5; xung_com_5 = 0; count_5 = 0; T5 = 8; t_count = 0; t_command = 0; t_quyhoach_5 = 0; error_5 = 0; //========================================================== ==== //========================================================== ==== Serial.begin(115200); //khoi tao gui thong tin serial Timer1.attachInterrupt(dieu_khien_1); Timer2.attachInterrupt(dieu_khien_2); Timer3.attachInterrupt(dieu_khien_3); Timer4.attachInterrupt(dieu_khien_4); Timer5.attachInterrupt(dieu_khien_5); Timer6.attachInterrupt(quy_hoach); // Chu ky 10ms } void phat_xung(unsigned int chan_phat_xung, unsigned int timedl) { digitalWrite(chan_phat_xung, HIGH); delayMicroseconds(timedl); 72 Khoa ĐT CLC – ĐH SPKT TP.HCM digitalWrite(chan_phat_xung, LOW); delayMicroseconds(timedl); } void forward(unsigned int chan_chieu) { digitalWrite(chan_chieu, HIGH); } void reverse(unsigned int chan_chieu) { digitalWrite(chan_chieu, LOW); } void Stop(unsigned int chan_chieu, unsigned int chan_phat_xung) { digitalWrite(chan_chieu, LOW); digitalWrite(chan_phat_xung, LOW); } void dieu_khien_1() { if ((the_1_flex - the_1_pre) > 0) { if (count_1 < round(the_1_flex - the_1_pre)) { forward(DIR_1); phat_xung(PULSE_1, T1); count_1++; vitri_thuc_1 = vitri_thuc_1 + 0.01; } else { Stop(DIR_1, PULSE_1); Timer1.stop(); } } else if ((the_1_flex - the_1_pre) < 0) { if (count_1 < abs(round(the_1_flex - the_1_pre))) { reverse(DIR_1); phat_xung(PULSE_1, T1); 73 Khoa ĐT CLC – ĐH SPKT TP.HCM count_1++; vitri_thuc_1 = vitri_thuc_1 - 0.01; } else { Stop(DIR_1, PULSE_1); Timer1.stop(); } } if (t_count > (t_command)) { offset_1(); } } void offset_1() { error_1 = int((vitri_dat_1 - vitri_thuc_1) * 100); if (error_1 > 0) { forward(DIR_1); phat_xung(PULSE_1, T1); vitri_thuc_1 = vitri_thuc_1 + 0.01; } else if (error_1 < 0) { reverse(DIR_1); phat_xung(PULSE_1, T1); vitri_thuc_1 = vitri_thuc_1 - 0.01; } } void dieu_khien_2() { if ((the_2_flex - the_2_pre) > 0) { if (count_2 < round(the_2_flex - the_2_pre)) { forward(DIR_2); phat_xung(PULSE_2, T2); count_2++; vitri_thuc_2 = vitri_thuc_2 + 0.01; } 74 Khoa ĐT CLC – ĐH SPKT TP.HCM else { Stop(DIR_2, PULSE_2); Timer2.stop(); } } else if ((the_2_flex - the_2_pre) < 0) { if (count_2 < abs(round(the_2_flex - the_2_pre))) { reverse(DIR_2); phat_xung(PULSE_2, T2); count_2++; vitri_thuc_2 = vitri_thuc_2 - 0.01; } else { Stop(DIR_2, PULSE_2); Timer2.stop(); } } if (t_count > (t_command)) { offset_2(); } } void offset_2() { error_2 = int((vitri_dat_2 - vitri_thuc_2) * 100); if (error_2 > 0) { forward(DIR_2); phat_xung(PULSE_2, T2); vitri_thuc_2 = vitri_thuc_2 + 0.01; } else if (error_2 < 0) { reverse(DIR_2); phat_xung(PULSE_2, T2); vitri_thuc_2 = vitri_thuc_2 - 0.01; } } 75 Khoa ĐT CLC – ĐH SPKT TP.HCM void dieu_khien_3() { if ((the_3_flex - the_3_pre) > 0) { if (count_3 < round(the_3_flex - the_3_pre)) { forward(DIR_3); phat_xung(PULSE_3, T3); count_3++; vitri_thuc_3 = vitri_thuc_3 + 0.01; } else { Stop(DIR_3, PULSE_3); Timer3.stop(); } } else if ((the_3_flex - the_3_pre) < 0) { if (count_3 < abs(round(the_3_flex - the_3_pre))) { reverse(DIR_3); phat_xung(PULSE_3, T3); count_3++; vitri_thuc_3 = vitri_thuc_3 - 0.01; } else { Stop(DIR_3, PULSE_3); Timer3.stop(); } } if (t_count > (t_command)) { offset_3(); } } void offset_3() { error_3 = int((vitri_dat_3 - vitri_thuc_3) * 100); if (error_3 > 0) 76 Khoa ĐT CLC – ĐH SPKT TP.HCM { forward(DIR_3); phat_xung(PULSE_3, T3); vitri_thuc_3 = vitri_thuc_3 + 0.01; } else if (error_3 < 0) { reverse(DIR_3); phat_xung(PULSE_3, T3); vitri_thuc_3 = vitri_thuc_3 - 0.01; } } void dieu_khien_4() { if ((the_4_flex - the_4_pre) > 0) { if (count_4 < round(the_4_flex - the_4_pre)) { forward(DIR_4); phat_xung(PULSE_4, T4); count_4++; vitri_thuc_4 = vitri_thuc_4 + 0.01; } else { Stop(DIR_4, PULSE_4); Timer4.stop(); } } else if ((the_4_flex - the_4_pre) < 0) { if (count_4 < abs(round(the_4_flex - the_4_pre))) { reverse(DIR_4); phat_xung(PULSE_4, T4); count_4++; vitri_thuc_4 = vitri_thuc_4 - 0.01; } else { Stop(DIR_4, PULSE_4); Timer4.stop(); 77 Khoa ĐT CLC – ĐH SPKT TP.HCM } } if (t_count > (t_command)) { offset_4(); } } void offset_4() { error_4 = int((vitri_dat_4 - vitri_thuc_4) * 100); if (error_4 > 0) { forward(DIR_4); phat_xung(PULSE_4, T4); vitri_thuc_4 = vitri_thuc_4 + 0.01; } else if (error_4 < 0) { reverse(DIR_4); phat_xung(PULSE_4, T4); vitri_thuc_4 = vitri_thuc_4 - 0.01; } } void dieu_khien_5() { if ((the_5_flex - the_5_pre) > 0) { if (count_5 < round(the_5_flex - the_5_pre)) { forward(DIR_5); phat_xung(PULSE_5, T5); count_5++; vitri_thuc_5 = vitri_thuc_5 + 0.01; } else { Stop(DIR_5, PULSE_5); Timer5.stop(); } } else if ((the_5_flex - the_5_pre) < 0) 78 Khoa ĐT CLC – ĐH SPKT TP.HCM { if (count_5 < abs(round(the_5_flex - the_5_pre))) { reverse(DIR_5); phat_xung(PULSE_5, T5); count_5++; vitri_thuc_5 = vitri_thuc_5 - 0.01; } else { Stop(DIR_5, PULSE_5); Timer5.stop(); } } if (t_count > (t_command)) { offset_5(); } } void offset_5() { error_5 = int((vitri_dat_5 - vitri_thuc_5) * 100); if (error_5 > 0) { forward(DIR_5); phat_xung(PULSE_5, T5); vitri_thuc_5 = vitri_thuc_5 + 0.01; } else if (error_5 < 0) { reverse(DIR_5); phat_xung(PULSE_5, T5); vitri_thuc_5 = vitri_thuc_5 - 0.01; } } void serialEvent() { String test1 = ""; String test2 = ""; String test3 = ""; String test4 = ""; 79 Khoa ĐT CLC – ĐH SPKT TP.HCM String test5 = ""; String test6 = ""; if (Serial.available() > 0) { test1 = Serial.readStringUntil(','); test2 = Serial.readStringUntil(','); test3 = Serial.readStringUntil(','); test4 = Serial.readStringUntil(','); test5 = Serial.readStringUntil(','); test6 = Serial.readStringUntil('\n'); vitri_dat_1 = test1.toFloat(); //Goc quay mong muon vitri_dat_2 = test2.toFloat(); vitri_dat_3 = test3.toFloat(); vitri_dat_4 = test4.toFloat(); vitri_dat_5 = test5.toFloat(); t_command = test6.toFloat(); // Thoi gian di chuyen mong muon xung_com_1 = int((vitri_dat_1 - vitri_thuc_1) * 100); the_1 = xung_com_1; xung_com_2 = int((vitri_dat_2 - vitri_thuc_2) * 100); the_2 = xung_com_2; xung_com_3 = int((vitri_dat_3 - vitri_thuc_3) * 100); the_3 = xung_com_3; xung_com_4 = int((vitri_dat_4 - vitri_thuc_4) * 100); the_4 = xung_com_4; xung_com_5 = int((vitri_dat_5 - vitri_thuc_5) * 100); the_5 = xung_com_5; /* Reset thoi gian bat dau qui hoach quy dao khop -*/ t_count = 0; the_1_pre = 0; the_1_flex = 0; count_1 = 0; a1 = the_1_pre; b1 = 0; c1 = * (the_1 - the_1_pre) / (t_command * t_command); d1 = -2 * (the_1 - the_1_pre) / (t_command * t_command * t_command); /* Ket thuc ceset thoi gian bat dau qui hoach quy dao -*/ 80 Khoa ĐT CLC – ĐH SPKT TP.HCM /* Reset thoi gian bat dau qui hoach quy dao khop -*/ t_count = 0; the_2_pre = 0; the_2_flex = 0; count_2 = 0; a2 = the_2_pre; b2 = 0; c2 = * (the_2 - the_2_pre) / (t_command * t_command); d2 = -2 * (the_2 - the_2_pre) / (t_command * t_command * t_command); /* Ket thuc ceset thoi gian bat dau qui hoach quy dao -*/ /* Reset thoi gian bat dau qui hoach quy dao khop -*/ t_count = 0; the_3_pre = 0; the_3_flex = 0; count_3 = 0; a3 = the_3_pre; b3 = 0; c3 = * (the_3 - the_3_pre) / (t_command * t_command); d3 = -2 * (the_3 - the_3_pre) / (t_command * t_command * t_command); /* Ket thuc ceset thoi gian bat dau qui hoach quy dao -*/ /* Reset thoi gian bat dau qui hoach quy dao khop -*/ t_count = 0; the_4_pre = 0; the_4_flex = 0; count_4 = 0; a4 = the_4_pre; b4 = 0; c4 = * (the_4 - the_4_pre) / (t_command * t_command); d4 = -2 * (the_4 - the_4_pre) / (t_command * t_command * t_command); /* Ket thuc ceset thoi gian bat dau qui hoach quy dao -*/ /* Reset thoi gian bat dau qui hoach quy dao khop -*/ t_count = 0; the_5_pre = 0; the_5_flex = 0; count_5 = 0; 81 Khoa ĐT CLC – ĐH SPKT TP.HCM a5 = the_5_pre; b5 = 0; c5 = * (the_5 - the_5_pre) / (t_command * t_command); d5 = -2 * (the_5 - the_5_pre) / (t_command * t_command * t_command); /* Ket thuc ceset thoi gian bat dau qui hoach quy dao -*/ Timer6.start(10000); // Kich hoach qui hoach quy dao } } void quy_hoach() // Chuong trinh phuc vu ngat cho Timer (10ms) { // t_count = t_count + 0.01; // Tang thoi gian dem (buoc 10ms) the_1_pre = the_1_flex; //Luu gia tri truoc the_2_pre = the_2_flex; the_3_pre = the_3_flex; the_4_pre = the_4_flex; the_5_pre = the_5_flex; if (t_count < t_command) { t_count = t_count + 0.01; // Tang thoi gian dem (buoc 10ms) /*========================================================== =========================================================== ========*/ the_1_flex = a1 + b1 * t_count + c1 * (t_count * t_count) + d1 * (t_count * t_count * t_count); // Quy hoach quy dao count_1 = 0; // Reset so xung da goi if ((the_1_flex - the_1_pre) == 0) t_quyhoach_1 = 10000; else t_quyhoach_1 = round(abs(5000 / (the_1_flex - the_1_pre))); // Timer1.start(t_quyhoach_1); /*========================================================== =========================================================== ========*/ the_2_flex = a2 + b2 * t_count + c2 * (t_count * t_count) + d2 * (t_count * t_count * t_count); // Quy hoach quy dao count_2 = 0; // Reset so xung da goi if ((the_2_flex - the_2_pre) == 0) t_quyhoach_2 = 10000; else t_quyhoach_2 = round(abs(5000 / (the_2_flex - the_2_pre))); // Timer2.start(t_quyhoach_2); 82 Khoa ĐT CLC – ĐH SPKT TP.HCM /*========================================================== =========================================================== ========*/ the_3_flex = a3 + b3 * t_count + c3 * (t_count * t_count) + d3 * (t_count * t_count * t_count); // Quy hoach quy dao count_3 = 0; // Reset so xung da goi if ((the_3_flex - the_3_pre) == 0) t_quyhoach_3 = 10000; else t_quyhoach_3 = round(abs(5000 / (the_3_flex - the_3_pre))); // Timer3.start(t_quyhoach_3); /*========================================================== =========================================================== ========*/ the_4_flex = a4 + b4 * t_count + c4 * (t_count * t_count) + d4 * (t_count * t_count * t_count); // Quy hoach quy dao count_4 = 0; // Reset so xung da goi if ((the_4_flex - the_4_pre) == 0) t_quyhoach_4 = 10000; else t_quyhoach_4 = round(abs(5000 / (the_4_flex - the_4_pre))); // Timer4.start(t_quyhoach_4); /*========================================================== =========================================================== ========*/ the_5_flex = a5 + b5 * t_count + c5 * (t_count * t_count) + d5 * (t_count * t_count * t_count); // Quy hoach quy dao count_5 = 0; // Reset so xung da goi if ((the_5_flex - the_5_pre) == 0) t_quyhoach_5 = 10000; else t_quyhoach_5 = round(abs(5000 / (the_5_flex - the_5_pre))); // Timer5.start(t_quyhoach_5); /*========================================================== =========================================================== ========*/ Timer1.start(t_quyhoach_1); Timer2.start(t_quyhoach_2); Timer3.start(t_quyhoach_3); Timer4.start(t_quyhoach_4); Timer5.start(t_quyhoach_5); } if (t_count > (t_command)) { 83 Khoa ĐT CLC – ĐH SPKT TP.HCM Timer6.stop(); } } void serial_full() { if (Serial.availableForWrite() > 20) { Serial.print(vitri_dat_1); Serial.print(" === "); Serial.print(vitri_thuc_1); Serial.print(" === "); Serial.print(the_1_flex); Serial.print(" === "); Serial.print(the_1_pre); Serial.print(" === "); Serial.print(vitri_dat_2); Serial.print(" === "); Serial.print(vitri_thuc_2); Serial.print(" === "); Serial.print(the_2_flex); Serial.print(" === "); Serial.print(the_2_pre); Serial.print(" === "); Serial.print(vitri_dat_3); Serial.print(" === "); Serial.print(vitri_thuc_3); Serial.print(" === "); Serial.print(the_3_flex); Serial.print(" === "); Serial.print(the_3_pre); Serial.print(" === "); Serial.print(vitri_dat_4); Serial.print(" === "); Serial.print(vitri_thuc_4); Serial.print(" === "); Serial.print(the_4_flex); Serial.print(" === "); Serial.print(the_4_pre); Serial.print(" === "); Serial.print(vitri_dat_5); Serial.print(" === "); Serial.print(vitri_thuc_5); 84 Khoa ĐT CLC – ĐH SPKT TP.HCM Serial.print(" === "); Serial.print(the_5_flex); Serial.print(" === "); Serial.print(the_5_pre); Serial.print(" === "); Serial.print(t_count); Serial.print(" === "); Serial.println(t_command); } } void serial_C_sharp() { if (Serial.availableForWrite() > 20) { Serial.print(vitri_dat_1); Serial.print(" === "); Serial.print(vitri_thuc_1); Serial.print(" === "); Serial.print(vitri_dat_2); Serial.print(" === "); Serial.print(vitri_thuc_2); Serial.print(" === "); Serial.print(vitri_dat_3); Serial.print(" === "); Serial.print(vitri_thuc_3); Serial.print(" === "); Serial.print(vitri_dat_4); Serial.print(" === "); Serial.print(vitri_thuc_4); Serial.print(" === "); Serial.print(vitri_dat_5); Serial.print(" === "); Serial.println(vitri_thuc_5); } } void loop() { serial_C_sharp(); //Goi du lieu len C sharp } 85 Khoa ĐT CLC – ĐH SPKT TP.HCM S K L 0 ... em thiết kế, chế tạo điều khiển robot làm việc cách tồn diện khơng gian Với đề tài đồ án tốt nghiệp “THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BẬC TỰ DO KHÔNG RÀNG BUỘC SỬ DỤNG KỸ THUẬT ĐỘNG HỌC NGHỊCH THÔNG...TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO TẠO CHẤT LƯỢNG CAO ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ VÀ ĐIỀU KHIỂN ROBOT BẬC TỰ DO KHÔNG RÀNG BUỘC SỬ DỤNG KỸ THUẬT ĐỘNG HỌC NGHỊCH... Toán SLM 31 Biểu Đồ 3.5 Quá Trình Hội Tụ Của Sai Số Các Khớp Với Thuật Toán SLM 32 Biểu Đồ 3 .6 Biến Đổi Thông Minh Của Các Thông Số Học Với Thuật Toán SLM 32 Biểu Đồ 3.7 Kết Quả Động Học Nghịch