(Đồ án hcmute) 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

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH ĐỒ ÁN TỐT NGHIỆP NGÀNH CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA 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 GVHD: TS ĐẶNG XUÂN BA SVTH: ĐẶNG SỸ BÌNH MSSV: 16151002 SKL 0 7 Tp Hồ Chí Minh, tháng 08/2020 an 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 an 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 an CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc *** -Tp Hồ Chí Minh, ngày 25 tháng năm 2020 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên : Đặng Sỹ Bình MSSV: 16151002 Ngành: Cơng nghệ kỹ thuật Điều khiển Tự động hóa Lớp: 16151CL2 Giảng viên hướng dẫn: TS Đặng Xuân Ba ĐT: 0945 853 990 Ngày nhận đề tài: 01 / 02 / 2020 Ngày nộp đề tài: 25 / / 2020 Tên đề tài: Thiết kế điều khiển robot bậc tự không ràng buộc sử dụng kỹ thuật động học nghịch thông minh Các số liệu, tài liệu ban đầu: Nội dung thực đề tài: Tính tốn thiết kế phần cứng mơ hình UR3 Tìm hiểu giải thuật mơ cân cho mơ hình phần mềm Matlab Áp dụng giải thuật điều khiển cho mơ hình thực, quy hoạch quỹ đạo để robot hoạt động ổn định tối ưu Sản phẩm: Mơ hình thực nghiệm robot bậc Chương trình điều khiển giám sát Quyển báo cáo đồ án tốt nghiệp Đĩa CD TRƯỞNG NGÀNH GIẢNG VIÊN HƯỚNG DẪN i an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh Phúc PHIẾU NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN Họ tên sinh viên : Đặng Sỹ Bình MSSV: 16151002 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế điều khiển robot bậc tự không ràng buộc sử dụng kỹ thuật động học nghịch thông minh Họ tên Giáo viên hướng dẫn: TS Đặng Xuân Ba NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Đề tài hướng tới phát triển dạng robot bậc tự có cấu hình sử dụng thuật tốn điều khiển động học thơng minh, nội dung cơng việc lớn bao gồm tính tốn, mô phỏng, thiết kế, thi công điều khiển giám sát hệ thống……………… Ưu điểm: Để thực thành cơng đồ án, địi hỏi lượng cơng việc lớn địi hỏi kỹ chun sâu kỹ thuật robot, nhóm hồn thành tốt mục tiêu đề làm việc chăm bền bỉ đam mê sáng tạo công việc……… Khuyết điểm: Do thời gian thực công việc ngắn lượng cơng việc lớn, nên nhóm chưa kịp khắc phục nhược điểm nhỏ thiết kế, chế tạo robot…………………… Đề nghị cho bảo vệ hay không? Được bảo vệ Đánh giá loại: Xuất sắc Điểm: … 9.5…………… (Bằng chữ: Chín rưỡi … ) Tp Hồ Chí Minh, ngày 25 tháng năm 2020 Giáo viên hướng dẫn (Ký & ghi rõ họ tên) ii an CỘNG HOÀ XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc Lập – Tự Do – Hạnh Phúc BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SPKT TP.HCM PHIẾU NHẬN XÉT PHẢN BIỆN Họ tên sinh viên :…Đặng Sỹ Bình Tên đề tài : 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 Giảng viên Hướng dẫn : TS ĐẶNG XUÂN BA Giảng viên Phản biện : PGS.TS LÊ MỸ HÀ Ý KIẾN NHẬN XÉT Nhận xét chung nội dung đề tài Nội dung đủ hàm lượng Đồ Án Tốt Nghiệp Đại học Ý kiến kết luận (ghi rõ nội dung cần bổ sung, hiệu chỉnh) Chỉnh sửa lỗi tả, lỗi đánh máy Bổ sung kết thực nghiệm Đề nghị : Được bảo vệ:  Bổ sung để bảo vệ:  Không bảo vệ:  Câu hỏi phản biện (Giảng viên khơng cho SV biết trước) 1/ Phân tích sở tốn học việc thêm thơng số γ phương trình 3.14 ? 2/ Đánh giá độ xác kết thực nghiệm ? 3/ Điều kiện dừng tìm thơng số góc Ɵ cơng thức 3.14 ? Điểm đánh giá đề tài: /10 (Bằng chữ: tám ) TP HCM, ngày tháng năm 2020 Người nhận xét iii an 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 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 chun 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 cô 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 an 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 tố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 an 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 an 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 an 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 an 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 an 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 an 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 an 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 an { 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 an } } 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 an { 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 an 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 an /* 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 an 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 an /*========================================================== =========================================================== ========*/ 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 an 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 an 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 an S an 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... viên : Đặng Sỹ Bình MSSV: 161 51002 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế điều khiển robot bậc tự không ràng buộc sử dụng kỹ thuật động học nghịch thông minh Họ tên Giáo viên hướng... Tên đề tài: Thiết kế điều khiển robot bậc tự không ràng buộc sử dụng kỹ thuật động học nghịch thông minh Các số liệu, tài liệu ban đầu: Nội dung thực đề tài: Tính toán thiết kế phần cứng