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 SKL007174 Tp Hồ Chí Minh, tháng 08/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 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 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 toá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 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 MSSV: 16151002 Họ tên sinh viên : Đặng Sỹ Bình 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ỹ chuyên 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 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐH SPKT TP.HCM Họ tên sinh viên Tên đề tài Giảng viên Hướng dẫn Giảng viên Phản biện 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 chính 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ở toán học việc thêm thơng số γ phương trình 3.14 ? 2/ Đánh giá độ chính 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: iii 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 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 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 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 toá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 .2 1.2 Tổng quan robot UR3 robot 1.2.1 Sơ lược lịch sử robot .3 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 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(); vitri_dat_2 = test2.toFloat(); vitri_dat_3 = test3.toFloat(); vitri_dat_4 = test4.toFloat(); vitri_dat_5 = test5.toFloat(); t_command = test6.toFloat(); //Goc quay mong muon // 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) //Luu gia tri truoc the_1_pre = the_1_flex; 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 ... MSSV: 161 51002 Họ tên sinh viên : Đặng Sỹ Bình 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ọ... 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... 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