Đang tải... (xem toàn văn)
Untitled THÀNH PHỐ HỒ CHÍ MINH BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT MSSV Tp Hồ Chí Minh, tháng 20 SKL 0 0 7 1 8 6 TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH KHOA ĐÀO T[.]
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 ĈӖÈ17Ӕ71*+,ӊ3 1*¬1+&1.7Ĉ,ӄ8.+,ӆ19$Ғ7ӴĈӜ1*+Ï$ 7+,7.9$&+7252%27 %ặ&7'27,&++3%,8 +,17+é1*0,1+ *9+' 761*;8ặ1%$ 697+ 9đ717$, MSSV 697+ 2$11*&0,1+ 0669 SKL 0 Tp Hồ Chí Minh, tháng 20 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À CHẾ TẠO ROBOT BẬC TỰ DO TÍCH HỢP BỘ ĐIỀU KHIỂN THÔNG MINH SVTH : VÕ TẤN TÀI MSSV : 16151079 SVTH : ĐOÀN NGỌC MINH MSSV : 16151047 KHÓA : 2016 - 2020 NGÀNH : CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD : TS ĐẶNG XUÂN BA Tp 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À CHẾ TẠO ROBOT BẬC TỰ DO TÍCH HỢP BỘ ĐIỀU KHIỂN THÔNG MINH SVTH : VÕ TẤN TÀI MSSV : 16151079 SVTH : ĐOÀN NGỌC MINH MSSV : 16151047 NGÀNH : CNKT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA GVHD : TS ĐẶNG XUÂN BA Tp 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 27 tháng năm 2020 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên 1: Võ Tấn Tài MSSV: 16151079 Họ tên sinh viên 2: Đoàn Ngọc Minh MSSV: 16151047 Ngành: CNKT Đ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: 24 / 02 / 2020 Ngày nộp đề tài: 27 / / 2020 Tên đề tài: Thiết kế chế tạo robot bậc tự tích hợp điều khiển 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 robot bậc tự Tìm hiểu giải thuật mơ cho mơ hình robot bậc tự phần mềm Matlab Áp dụng giải thuật điều khiển cho mơ hình thực, hiệu chỉnh để giảm sai số, thời gian xác lập, độ vọt lố Sản phẩm: Mơ hình thực nghiệm robot bậc tự 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 Họ tên sinh viên 1: Võ Tấn Tài MSSV: 16151079 Họ tên sinh viên 2: Đoàn Ngọc Minh MSSV: 16151047 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế chế tạo robot bậc tự tích hợp điều khiển 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 robot bậc tự sử dụng thuật tốn điều khiển 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: Mặc dù khối lượng cơng việc lớn địi hỏi độ chun sâu kỹ thuật robot, nhóm hồn thành tốt mục tiêu đề cố gắng nỗ lực cao độ thân nhạy bén sáng tạo công việc Khuyết điểm: Nhóm thực đồ án làm quen tiếp xúc với robot thời gian tương đối ngắn, nên khơng tránh khỏi thiếu sót nhược điểm thiết kế chế tạo robot phức tạp Đề nghị cho bảo vệ hay không? Được bảo vệ Đánh giá loại: Xuất sắc Điểm: … ……………… (Bằng chữ: … ) Tp Hồ Chí Minh, ngày 27 tháng năm 2020 Giáo viên hướng dẫn (Ký & ghi rõ họ tên) ii 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 PHẢN BIỆN Họ tên sinh viên 1: Võ Tấn Tài MSSV: 16151079 Họ tên sinh viên 2: Đoàn Ngọc Minh MSSV: 16151047 Ngành: CNKT Điều khiển tự động hóa Tên đề tài: Thiết kế chế tạo robot bậc tự tích hợp điều khiển thông minh Họ tên Giáo viên phản biện: NHẬN XÉT Về nội dung đề tài & khối lượng thực hiện: Ưu điểm: Khuyết điểm: Đề nghị cho bảo vệ hay không? Đánh giá loại: Điểm:……………….(Bằng chữ: ) Tp Hồ Chí Minh, ngày 27 tháng năm 2020 Giáo viên phản biện (Ký & ghi rõ họ tên) 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 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 bạn bè ln động viên, ủng hộ giúp đỡ chúng em lúc chúng em gặp khó khăn việc làm đồ án tốt nghiệp, để chúng em hoàn thành đồ án ngày hôm 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 2020 Sinh viên thực Võ Tấn Tài Đồn Ngọc Minh iv TĨM TẮT ĐỒ ÁN Dựa vào kiến thức học trường điều khiển tự động sự tìm hiểu ngành cơng nghiệp robot Chúng em nhận thấy nghiên cứu robot lĩnh vực quan tâm ngày Các 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 định tập trung hướng nghiên cứu giải thuật học để điều khiển chính xác robot bậc tự do, đồng thời thử nghiệm mơ hình thực tế Với đề tài đồ án tốt nghiệp “Thiết kế chế tạo robot bậc tự tích hợp điều khiển thông minh”, chúng em thực nội dung sau: Mô điều khiển robot bậc tự phần mềm Matlab 2018b Thiết kế chế tạo mơ hình thực nghiệm robot bậc tự Điều khiển chính xác vị trí robot bậc tự So sánh giải thuật điều khiển PID PD Folding Nhóm tiến hành tính tốn, mô động học động lực học, áp dụng giải thuật điều khiển, đồng thời nghiên cứu chế tạo mơ hình robot bậc tự Sau mơ hình nhiệm vụ mơ hồn thành, chúng em tiến hành áp dụng giải thuật điều khiển nghiên cứu lên mơ hình thực v ABSTRACT Based on knowledge studied on automatic control theories and robotics during the undergraduate course, we found that research on robotics is one of the most interested areas today So far, automatic robot, semi-automatic robot or robots with artificial intelligence have been increasingly popular Catching the trend of the automatic control research, we decided to choose a robot-related graduation project Our specialized knowledge is however limited, and the field of robots is very complex and advanced So, our project has mainly focused on intensive algorithms for precise control of a degree-of-freedom robot In order to finish the project titled: "Design and manufacture of degrees of freedom robots integrated intelligent controller", we have implemented the following contents: Modeling and control a 6-DOF robot on Matlab 2018b software Designing and fabricating a real 6-DOF robot prototype Applying the proposed controller on the real robot Verifying the working performance of the designed system by real-time experiments in a comparison of PID and PD folding controllers Our team calculated and simulated kinematics and dynamics, the applied the proposed control algorithms on simulation environment At the same time, we studied and built a robot 6-DOF model After the model and simulation tasks were completed, we proceed to apply the control algorithms studied to the real robot vi MỤC LỤC Nhiệm vụ đồ án tốt nghiệp i Trang phiếu nhận xét giáo viên hướng dẫn ii Trang phiếu nhận xét giáo viên phản biện iii Lời cảm ơn iv Tóm tắt v Mục lục vii Danh mục chữ viết tắt xi Danh mục bảng biểu xii Danh mục hình ảnh, biểu đồ xiii CHƯƠNG TỔNG QUAN ĐỀ TÀI 1.1 Tính cấp thiết đề tài 1.2 Tổng quan robot Robot bậc tự 1.2.1 Sơ lược lịch sử Robot 1.2.2 Tổng quan Robot bậc tự 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 12 1.6 Phương pháp nghiên cứu 12 1.7 Kết cấu luận văn 12 CHƯƠNG CƠ SỞ LÝ THUYẾT 13 vii Mơ hình thực nghiệm cho kết mơ hình 5.41 5.42, hệ thống đáp ứng có sự khác biệt rõ rệt phương pháp điều khiển Phương pháp PD Folding bám giá trị điểm đặt tốt rõ rệt Hệ thống phương pháp PD Folding có thời gian xác lập nhanh Đồng thời, phương pháp PD Folding sai số xấp xỉ bằng 0, phương pháp PID, sai số hệ thống mức 0.5 độ 5.3.3 Kết luận Qua kiểm chứng mô lẫn thực nghiệm cho thấy phương pháp điều khiển thơng minh PD Folding có nhiều ưu vượt trội so với phương pháp PID truyền thống: Thời gian xác lập ngắn hơn, sai số ít hơn, độ vọt lố thấp hơn, hệ thống ổn định Với minh chứng trên, thấy rằng phương pháp điều khiển PD Folding ứng dụng vào điều khiển hệ thống đơn giản lẫn phức tạp Hầu hết công ty sử dụng phương pháp điều khiển truyền thống PID, ưu phương pháp điều khiển PD Folding thay nhiệm vụ mà PID đảm nhiệm để đem lại kết điều khiển tốt hơn, chính xác hơn, nhanh chóng 86 CHƯƠNG KẾT QUẢ ĐẠT ĐƯỢC VÀ HƯỚNG PHÁT TRIỂN 6.1 Kết đạt Tính tốn, mơ động học Robot, xây dựng hình dáng robot mô Simulink Matlab Phát triển phương trình động lực học mơ hành vi Robot 6-DOF không gian chiều Áp dụng thuật toán PID PD Folding vào điều khiển mơ để điều khiển xác vị trí Thiết kế, chế tạo lắp ráp hồn thiện mơ hình thực nghiệm Robot 6-DOF Thử nghiệm thuật tốn điều khiển cân bằng PID PD Folding để điều khiển mơ hình Robot 6-DOF thực nghiệm Điều khiển khớp Robot 6-DOF xác vị trí cho trước Thực thiết kế giao diện giám sát điều khiển Winform C# Đã nghiên cứu sử dụng Board vi điều khiển Arduino kết hợp với ngoại vi Nghiên cứu điều khiển động kèm driver J2S 10A hãng Mitsubishi 6.2 Hướng phát triển Nhận dạng mơ hình thực nghiệm Robot 6-DOF Thử nghiệm thuật toán điều khiển khác như: điều khiển tối ưu LQR, PID Fuzzy, điều khiển trượt Thiết kế hệ thống lọc nhiễu cho hệ thống đo góc Thiết kế lại phần cứng, tinh giảm chi tiết, giúp Robot chuyển động linh hoạt Sử dụng chip xử lí, sử dụng PLC điều khiển để tiện lợi tăng độ xác 87 TÀI LIỆU THAM KHẢO Tài liệu tiếng Việt [1] Huỳnh Thái Hồng (2006), “Giáo trình Hệ thống điều khiển tự động”, Nhà xuất Đại học quốc gia TP Hồ Chí Minh, 421 trang [2] Huỳnh Thái Hồng (2014), “Giáo trình Hệ thống điều khiển thông minh”, Nhà xuất Đại học quốc gia TP Hồ Chí Minh, 421 trang [3] Dương Hồi Nghĩa (2011), “Giáo trình điểu khiển đa biến”, Nhà xuất Đại học quốc gia TP Hồ Chí Minh, 421 trang Tài liệu tiếng Anh [4] J J Craig, Introduction to Robotics: Mechanics and Control, 3rd ed, Pearson Prentice Hall, USA, 2005 [5] D X Ba, M-H Le “Gain-Learning Sliding Mode Control of Robot Manipulators with Time-Delay Estimation” in 2019 International Conference on System Science and Engineering (ICSSE), 2019 [6] H V A Truong, D T Tran, and K K Ahn, “A neural network based sliding mode control for tracking performance with parameters variation of a 3-dof manipulator,” Applied Sciences, vol 9, pp 2014-2023, 2019 [7] P Roco, “Stability of PID control for industrial robot arms,” IEEE Trans Robot Automation, vol 12, no 4, pp 606-614, 1996 [8] Y Su, P C Muller, C Zheng, “Global Asymptotic Saturated PID Control for Robot Manipulators,” IEEE Trans Control Syst Technol., vol 18, pp 1280– 1288,2010 [9] W Yim and S N Singl, “Feedback Linearization of Differential-Algebraic Systems and Force and Position Control of Manipulators,” In Proceedings of the 1993 American Control Conference, San Francisco, CA, USA, 2–4 June 1993; pp 2279–2283 [10] D T Tran, D X Ba and K K Ahn, “Adaptive Backstepping Sliding Mode Control for Equilibrium Position Tracking of an Electrohydraulic Elastic Manipulator,” IEEE Trans on Electron, May 2019 88 [11] D X Ba, H Yeom, J Kim, and J B Bae, “Gain-adaptive Robust Backstepping Position Control of a BLDC Motor System,” IEEE/ASME Trans on Mechatronics, August, 2018 [12] D X Ba, H Yeom, and J B Bae, “A Direct Robust Nonsingular Terminal Sliding Mode Controller based on an Adaptive Time-delay Estimator for Servomotor Rigid Robots,” Mechatronics, May 2019 [13] M Jin, J Lee, and N G Tsagarakis, “Model-free robust adaptive control of humanoid robots with flexible joints,” IEEE Trans Ind Electron., vol 64, no 2, pp 1706-1715, 2017 [14] J Baek, M Jin, and S Han, “A new adaptive sliding mode control scheme for application to robot manipulators,” IEEE Trans Ind Electron., vol 63, no 6, pp 3628-3637, 2016 [15] Vo, A.T.; Kang, H.-J An Adaptive Neural Non-Singular Fast-Terminal Sliding-Mode Control for Industrial Robotic Manipulators Appl Sci 2018, 8, 2562 [16] R J Wai and R Muthusamy, “Design of Fuzzy-Neural-Network-Inherited Backstepping Control for Robot Manipulator Including Actuator Dynamics,” IEEE Trans Fuzzy Syst 2014, 22, 709–722 [17] Le, T.D.; Kang, H.-J.; Suh, Y.-S.; Ro, Y.-S An online self-gain tuning method using neural networks for nonlinear PD computed torque controller of a 2-dof parallel manipulator Neurocomputing 2013, 116, 53–61 [18] S Skoczowski, S Domesk, K Pietrusewicz, and B Broel-Plater, “A method for improving the robustness of PID control,” IEEE Trans Ind Electron., vol 58, no 6, pp 1669-1676, 2005 [19] Y Pan, X Li, and H Yu, “Efficient PID tracking control of robotic manipulators driven by compliant actuators,” IEEE Trans On Control System Technology, vol 27, no 2, pp 915-922, 2019 89 [20] J L Meza, V Santibanez, S Soto, and M A Llama, “Fuzzy self-tuning PID semiglobal regulator for robot manipulators,” IEEE Trans Ind Electron., vol 59, no 6, pp 2709-2717, 2011 [21] Z Liu, G Lai, Y Zhang, and P Chen, “Adaptive fuzzy tracking control of nonlinear time-delay systems with dead–zone output mechanism based on a novel smooth model,” IEEE Trans Fuzzy Syst., vol 23, no 6, pp 1998–2011, 2015 [22] C Yang, Y Jiang, J Na, Z Li, L Cheng, and C Y Su, “Finite-time convergence adaptive fuzzy control for dual-arm robot with unknown kinematics and dynamics,” IEEE Trans Fuzzy Systems, vol 27, no 3, pp 574-588, 2019 [23] L Wang, T Chai, and L Zhai, “Neural-network-based terminal sliding-mode control of robotic manipulators including actuator dynamics,” IEEE Trans Ind Electron., vol 56, no 9, pp 3296-3304, 2009 [24] D X Ba, D Q Truong, and K K Ahn, “An integrated intelligent nonlinear control method for pneumatic artificial muscle,” IEEE/ASME Trans on Mechatronics, vol 21, no 4, 2016 [25] D.C.T Tu and K K Ahn, “Nonlinear PID control to improve the control performance of axes pneumatic artificial muscle manipulator using neural network,” Mechatronics, vol 16, pp 577–587, 2006 [26] D X Ba, K K Ahn, and N T Tai, “Adaptive Integral-type neural sliding mode control for pneumatic muscle actuator,” Int J Auto Tech, vol 8, no 6, pp 888-895, 2014 [27] T D Le, H J Kang, Y S Suh, and Y S Ro, “An online self-gain tuning method using neural networks for nonlinear PD computed torque controller of a 2-dof parallel manipulator,” Neurocomputing, vol 11,pp 53–61, 2013 [28] K.K Ahn and H T C Nguyen, “Intelligent switching control of a pneumatic muscle robot arm using learning vector quantization neural network,” Mechatronics, vol 17, pp 255–262, 2007 90 [29] Lewis FL, Abdallah CT, Dawson DM Control of robot manipulator New York: Macmillan; 1993 [30] S Yasui, “Stochastic functional fourier series, volterra series, and nonlinear systems analysis,” IEEE Trans Automatic Control, vol 24, no 2, pp 230-242, 1979 [31] S Abid, F Fnaiech and M Najim, “A fast feedforward training algorithm using a modified form of the standard backpropagation algorithm,” IEEE Trans Neural Network, vol 12, no 2, pp 424-430, 2001 [32] N Q Lin, D M Xuan, and D X Ba, “Advanced control design for a highprecision heating furnace using combination of PI/Neural network,” Journal of Technical Education Science, no 55, pp 25-31, 2020 91 PHỤ LỤC Phụ lục 1: Code đọc xung truyền xung qua chuẩn I2C: #include #include const byte SLAVE_ADDRESS = 42; #define A1 #define INB #define A2 #define INB2 int a2 = 0; int lbyte2 = 0; int hlbyte2 = 0; volatile long XUNG2=0; int a = 0; int lbyte = 0; int hlbyte = 0; volatile long XUNG1=0; double T; void setup() { Serial.begin(115200); Wire.begin(); pinMode(A1, INPUT_PULLUP); 92 pinMode(INB, INPUT_PULLUP); pinMode(A2, INPUT_PULLUP); pinMode(INB2, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(A2),demxung2,CHANGE); attachInterrupt(digitalPinToInterrupt(A1),demxung,CHANGE); } void demxung() { if (digitalRead(INB)==digitalRead(A1)) XUNG1 ; else XUNG1++; } void demxung2() { if (digitalRead(INB2)==digitalRead(A2)) XUNG2 ; else XUNG2++; } byte k; void loop() { if (k==4) 93 { TWCR=0; delay(4); Wire.begin(); } Wire.beginTransmission (SLAVE_ADDRESS); a = XUNG1; lbyte = abs(a) % 256; hlbyte = (abs(a) - lbyte) / 256; a2 = XUNG2; // transmit to device #1 lbyte2 = abs(a2) % 256; hlbyte2 = (abs(a2) - lbyte2) / 256; Wire.write((a > 0) ? : 0); Wire.write((byte)abs(hlbyte)); Wire.write((byte)lbyte); Wire.write((a2 > 0) ? : 0); Wire.write((byte)abs(hlbyte2)); Wire.write((byte)lbyte2); k=Wire.endTransmission (false); delay(4); } Phụ lục 2: Code phương pháp PID #include #include 94 #define DIRR1 #define DIRL1 double Upd1, vitri1, Udk1, vitri_dat1, E1; double pE1, dE1, T1, LAST_E1; double LAMDA1, signU1, signUg1, tanhU1; String THETA1 = " "; double XUNG1, iT1; byte sig1, hbyte1, lbyte1; byte count1 = 0; const byte MY_ADDRESS = 42; double LastXung1=0, LastXung2=0; byte sig2, hbyte2, lbyte2; byte count2 = 0; double Kp,Ki,Kd; //////////////////////////// double SETPOINT1=0; void setup() { Serial.begin(115200); Wire.begin (MY_ADDRESS); Wire.onReceive(receiveEvent); pinMode(DIRR1, OUTPUT); pinMode(DIRL1, OUTPUT); pinMode(DAC0, OUTPUT); 95 analogWriteResolution(10); T1 = 0.01; Kp=12; Ki=0.001; Kd=0.3; LAST_E1 = 0; E1 = 0; XUNG1 = 0; Udk1 = 0; Timer1.attachInterrupt(BDK).start(T1 * 1000000); } void BDK() { vitri1 = XUNG1*360/720; E1 = SETPOINT1 - vitri1; pE1 = E1; dE1 = (E1 - LAST_E1) / T1; iT1 = E1 + LAST_E1* T1; LAST_E1 = E1; // DIEN AP DIEU KHIEN/// Udk1 = Kp*pE1 + Ki* iT1 + Kd* dE1; if ( abs(Udk1) < 10) Udk1 = 0; motor1(); 96 } void motor1() { if (Udk1 > 0) { analogWrite(DAC0, abs(Udk1)); digitalWrite(DIRR1, HIGH); digitalWrite(DIRL1, LOW); } else if (Udk1 < 0) { analogWrite(DAC0, abs(Udk1)); digitalWrite(DIRR1, LOW); digitalWrite(DIRL1, HIGH); } else { analogWrite(DAC0, 0); digitalWrite(DIRR1, LOW); digitalWrite(DIRL1, LOW); } } void receiveEvent (int howMany) { 97 count1 = 0; while (Wire.available()) { if (count1 == 0) { sig1 = Wire.read(); // receive a byte as character count1++; } else if (count1 == 1) { hbyte1 = Wire.read(); // receive a byte as character count1++; } else if(count1==2) { lbyte1 = Wire.read(); count1++; }else if(count1==3) { sig2 = Wire.read(); // receive a byte as character count1++; } else if (count1 == 4) { hbyte2 = Wire.read(); // receive a byte as character count1++; 98 } else lbyte2 = Wire.read(); } LastXung1=XUNG1; XUNG1 = (hbyte1 * 256 + lbyte1) * ((sig1 == 1) ? : -1); if(abs(XUNG1-LastXung1)>100) XUNG1=LastXung1; } void loop() { if (Serial.available() > 0) { THETA1 = Serial.readStringUntil('\n'); } vitri_dat1 = THETA1.toInt(); SETPOINT1=(1/(0.2 + T1))*(0.2*SETPOINT1 + vitri_dat1*T1); if (Serial.availableForWrite() > 0) { Serial.println(vitri1); delay(10); } } 99 S K L 0