(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển thông minh(Đồ án tốt nghiệp) Thiết kế và chế tạo Robot 6 bậc tự do tích hợp bộ điều khiển 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À 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 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 Đoà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ơ hoà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 2.1 Ngun lí hoạt động 13 2.2 Phần mơ hình 13 2.2.1 Cơ sở ý tưởng mơ hình 2.3 Thuật tốn điều khiển 2.3.1 Sơ lược lí thuyết điều khiển tự động 14 14 14 2.3.1.1 Các khái niệm 14 2.3.1.2 Các phần tử hệ thống điều khiển tự động 15 2.3.1.3 Phân loại hệ thống điều khiển tự động 16 2.3.1.4 Các nguyên tắc điều khiển 17 2.3.2 Lý thuyết điều khiển sử dụng 17 2.3.2.1 Giới thiệu điều khiển PID 17 2.3.2.2 Giới thiệu điều khiển PD FOLDING 20 2.3.3 Phần mềm MATLAB 21 2.3.3.1 Giới thiệu MATLAB 22 2.3.3.2 Tổng quan cấu trúc liệu ứng dụng MATLAB 23 2.3.3.3 Hệ thống Matlab 23 CHƯƠNG KHẢO SÁT MƠ HÌNH TỐN, MÔ PHỎNG KIỂM CHỨNG 25 3.1 Bảng Denavit-Hartenberg (DH) 25 3.2 Động học vị trí robot bậc tự 26 3.2.1 Động học thuận 26 3.2.2 Động học nghịch 28 3.2.3 Mô kiểm chứng MATLAB 30 3.3 Phương trình động lực học 34 3.3.1 Phương trình động lực học dạng ma trận 34 3.3.2 Mô động lực học robot bậc tự MATLAB 35 CHƯƠNG THIẾT KẾ VÀ THI CÔNG MƠ HÌNH 4.1 Lựa chọn mơ hình Robot bậc tự 4.1.1 Thiết kế mơ hình dùng Solidworks 4.1.1.1 Sơ lược Solidworks 36 36 36 36 viii 4.1.1.2 Xuất vẽ 2D cắt CNC kim loại 37 4.1.1.3 Xuất file in 3D cho chi tiết 39 4.2 Chế tạo mơ hình Robot bậc tự 40 4.2.1 Khớp 41 4.2.2 Khớp 42 4.2.3 Khớp 43 4.2.4 Khớp 4, khớp khớp 43 4.2.5 Động AC Mitsubishi J2S 10A driver 45 4.3 Thiết kế phần mềm 46 4.3.1 Giới thiệu arduino 46 4.3.2 Giới thiệu phần mềm C# 50 4.3.3 Các thiết bị hỗ trợ xử lý: Opto quang, biến trở… 53 4.4 Phần khung số chi tiết khác CHƯƠNG ĐIỀU KHIỂN ROBOT BẬC TỰ DO 5.1 Thu thập liệu 5.1.1 Đọc xung Encoder 54 56 56 56 5.1.1.1 Thiết bị sử dụng 56 5.1.1.2 Kết nối phần cứng 56 5.1.1.3 Kết 57 5.2 Điều khiển mơ hình robot bậc tự 58 5.2.1 Thiết bị sử dụng 58 5.2.2 Kết nối phần cứng 58 5.2.3 Thiết kế điều khiển PD FOLDING robot bậc tự 59 5.2.3.1 Thiết kế mô Matlab 59 5.2.3.1.1 Thiết lập thông số mô Matlab 59 5.2.3.1.2 Mô phương pháp PD Folding ngõ vào hằng số 61 5.2.3.1.3 Mô phương pháp PD Folding ngõ vào sóng sin 65 5.2.3.2 Chương trình ARDUINO 71 5.2.3.3 Chương trình điều khiển thu thập liệu máy tính 71 ix 5.2.3.4 Tiến hành chạy mơ hình thực nghiệm 72 5.2.4 Thiết kế điều khiển PID robot bậc tự 74 5.2.4.1 Thiết kế mô Matlab 74 5.2.4.1.1 Mô phương pháp PID ngõ vào hằng số 75 5.2.4.1.2 Mô phương pháp PID ngõ vào sóng sin 77 5.2.4.2 Chương trình ARDUINO 79 5.2.4.3 Tiến hành chạy mơ hình thực 79 5.3 So sánh điều khiển PD FODING điều khiển PID 5.3.1 So sánh mô 80 80 5.3.1.1 So sánh phương pháp với điểm đặt mong muốn hằng số 80 5.3.1.2 So sánh phương pháp với điểm đặt mong muốn hàm sin 82 5.3.2 So sánh mơ hình thực nghiệm 85 5.3.3 Kết luận 86 CHƯƠNG KẾT QUẢ ĐẠT ĐƯỢC VÀ HƯỚNG PHÁT TRIỂN 87 6.1 Kết đạt 87 6.2 Hướng phát triển 87 TÀI LIỆU THAM KHẢO 88 PHỤ LỤC 92 x DANH MỤC CÁC TỪ VIẾT TẮT PID: Proportional Integral Derivative DOF: Degrees Of Freedom PD Folding: Proportional Derivative Folding HTTĐ: Hệ thống tự động ĐKTĐ: Điều khiển tự động DH: Denavit Hartenberg PEE : End-Effector Position xi 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