Tính toán động học, động lực học và điều khiển máy CNC 3 trục có cấu trúc

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI Vũ Trường Lam TÍNH TOÁN ĐỘNG HỌC, ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN MÁY CNC CÓ CẤU TRÚC ĐỘNG HỌC SONG SONG Chuyên ngành: Cơ Điện Tử LUẬN VĂN THẠC SĨ KHOA HỌC CÔNG NGHỆ CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC TS Nguyễn Hồng Thái Hà Nội - 2014 LỜI CAM ĐOAN Tên Vũ Trường Lam, học viên cao học khóa 2012B.CĐT.KH chuyên ngành Cơ Điện Tử Sau gần năm học tập, nghiên cứu trường Đại Học Bách Khoa Hà Nội, giúp đỡ thày cô giáo, đặc biệt TS Nguyễn Hồng Thái, hoàn thành xong luận văn tốt nghiệp thạc sĩ Với đề tài luận văn là: "Tính toán động học, động lực học điều khiển máy CNC trục có cấu trúc động học song song", xin cam đoan công trình nghiên cứu cá nhân dự hướng dẫn TS Nguyễn Hồng Thái tham khảo tài liệu liệt kê Tôi không chép công trình cá nhân khác hình thức Nếu có, xin hoàn thành chịu trách nhiệm Hà Nội, ngày 19 tháng 05 năm 2014 Người cam đoan Vũ Trường Lam ii LỜI CẢM ƠN Trong suốt trình thực đề tài "Tính toán động học, động lực học điều khiển máy CNC trục có cấu trúc động học song song", học viên đạt số kết định: Tìm hiểu cấu trúc máy song song kiểu trực giao Orthoglide gia công khí, tính toán động học, miền làm việc, động lực học điều khiển, thực mô gia công OpenGL, mô điều khiển MATLAB Simulink Học viên xin trân trọng cảm ơn giúp đỡ TS, Nguyễn Hồng Thái thày môn tạo điều kiện cho học viên hoàn thành đề tài Tuy nhiên, với nhiều khó khăn chuyên môn kinh nghiệm nên luận văn tránh thiết sót chưa thể hoàn thiện cách hoàn hảo mong đợi Kính mong quý thày cô đóng góp ý kiến để đề tài hoàn thành tốt Trường ĐH Bách Khoa Hà Nội HVTH Vũ Trường Lam iii MỤC LỤC LỜI CAM ĐOAN ii LỜI CẢM ƠN .iii MỤC LỤC iv DANH MỤC KÍ HIỆU vi DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ vii DANH MỤC CÁC BẢNG x DANH MỤC CÁC BẢNG x MỞ ĐẦU xi Chương TỔNG QUAN VỀ CƠ CẤU SONG SONG CÓ CẤU TRÚC TRỰC GIAO 1.1 LỊCH SỬ PHÁT TRIỂN CẤU TRÚC ĐỘNG HỌC SONG SONG 1.2 ỨNG DỤNG ROBOT SONG SONG TRONG CÔNG NGHIỆP VÀ CUỘC SỐNG 1.3 ROBOT SONG SONG ỨNG DỤNG TRONG GIA CÔNG 1.4 ROBOT SONG SONG TRỰC GIAO 1.4.1 Máy phay CNC trục sử dụng cấu trúc trực giao 1.4.2 Cơ cấu song song trực giao 1.4.3 Một số mô hình robot có sử dụng cấu trúc song song trực giao 1.5 CẤU TRÚC VÀ SƠ ĐỒ ĐỘNG HỌC CƠ CẤU TAY MÁY TRỰC GIAO 1.6 TÍNH ĐƯỜNG DỤNG CỤ TRONG GIA CÔNG BỀ MẶT KHÔNG GIAN TRÊN MÁY PHAY CNC TRỤC 11 1.6.1 Tính đường dụng cụ với dao phay ngón đầu cầu 11 1.6.2 Tính đường dụng cụ với dao đầu 12 1.6.3 Tính đường dụng cụ với dao đầu xuyến 14 1.6.4 Ví dụ áp dụng 15 Chương 17 PHÂN TÍCH ĐỘNG HỌC 17 2.1 BÀI TOÁN ĐỘNG HỌC THUẬN VÀ NGƯỢC 17 2.1.1 Mô tả cấu trúc máy 17 2.1.2 Đặt hệ trục tọa độ 18 2.1.3 Bài toán động học ngược 21 2.2 PHÂN TÍCH JACOBEAN 27 2.2.1 Cơ sở tính toán Jacobean 27 2.2.2 Xác định ma trận Jacobean 28 2.3 MIỀN LÀM VIỆC CỦA ROBOT 34 2.3.1 Cơ sở lý thuyết 34 2.3.2 Phương pháp hình học 35 2.3.3 Phương pháp giải tích 38 2.3.4 Tính toán miền làm việc 41 2.3.5 Thuật toán xác định miền biên 44 2.4 MÔ PHỎNG ĐỘNG HỌC BẰNG VC VÀ OPENGL 47 2.5 BÀI TOÁN VẬN TỐC 53 2.5.1 Vận tốc góc cánh tay 53 2.5.2 Vận tốc trọng tâm cánh tay 55 2.6 BÀI TOÁN GIA TỐC 56 2.6.1 Gia tốc góc cánh tay 56 iv 2.6.2 Gia tốc trọng tâm cánh tay 56 2.7 CÁC KẾT QUẢ ÁP DỤNG TRÊN MATLAB 57 Chương 61 ĐỘNG LỰC HỌC VÀ ĐIỀU KHIỂN 61 3.1 THIẾT LẬP PHƯƠNG TRÌNH ĐỘNG LỰC HỌC 61 3.1.1 Phương trình động lực học Lagrange 61 3.1.2 Các thông số động lực học 62 3.1.3 Hàm Lagrange hệ 66 3.1.4 Phương trình ràng buộc 66 3.1.5 Các đạo hàm phương trình Lagrange 67 3.1.6 Hệ phương trình vi phân máy 69 3.2 VÍ DỤ ÁP DỤNG 70 3.2.1 Cơ sở liệu đầu vào 70 3.2.2 Thông số động học 74 Chương 78 HỆ THỐNG ĐIỀU KHIỂN 78 4.1 CẤU TRÚC PHẦN CỨNG CỦA BỘ ĐIỀU KHIỂN CNC 78 4.1.1 Chức điều khiển cốt lõi NCK 78 4.1.2 Chức giao diện MMI 81 4.1.3 Cấu trúc chức điều khiển logic PLC 81 4.2 CẤU TRÚC PHẦN MỀM 82 4.3 CẤU TRÚC GCODE 85 4.4 XÂY DỰNG BỘ ĐIỀU KHIỂN 91 4.4.1 Giới thiệu điều khiển 91 4.4.2 Điều khiển PID 91 4.4.3 Điều khiển PD dựa vị trí vận tốc 92 4.4.4 Điều khiển PD với bù trọng trường 92 4.4.5 Điều khiển PD với phản hồi đầy đủ thông số động lực học 93 4.5 MÔ PHỎNG ĐIỀU KHIỂN 94 4.5.1 Giới thiệu sơ lược Matlab Simulink 94 4.5.2 Mô hình kết mô 94 KẾT LUẬN 101 I KẾT QUẢ ĐẠT ĐƯỢC CỦA LUẬN VĂN 101 II HƯỚNG PHÁT TRIỂN 101 TÀI LIỆU THAM KHẢO 102 PHỤ LỤC 104 A PHẦN MÔ PHỎNG LẬP TRÌNH GIA CÔNG 104 B CHƯƠNG TRÌNH XÁC ĐỊNH MIỀN LÀM VIỆC 106 C THUẬT TOÁN NỘI SUY GCODE 107 D THÔNG SỐ ĐỘNG LỰC HỌC TỪ SOLID WORKS 110 E CÁC HÀM TRONG MÔ PHỎNG ĐIỀU KHIỂN 113 F BẢN VẼ THỂ HIỆN CÁC THÔNG SỐ ĐỘNG HỌC MÁY 114 v DANH MỤC KÍ HIỆU Ký hiệu Nội dung, ý nghĩa  (Oxyz) Hệ tọa độ quy chiếu gốc máy (hệ 1) 1 ( Puvw) Hệ tọa độ động gắn tâm cụm trục (hệ 2) CLT i-1 Ri Điểm định vị dụng cụ Ma trận quay biến đổi tự hệ tọa độ i hệ i-1 Các thông số điều khiển (i=1, 2, 3) d i , θ i , β i ,i l1, l2 l5  si    Sx , S y , Sz   ω A i Bi , ω Bi C i   S A i Bi , S Bi C i   v M1i , v M 2i   ε A i B i , ε Bi C i   a M1i , a M 2i  g Chiều dài đoạn AiHi (i=1, 2, 3) Chiêu dài cánh tay AiBi, BiCi (i=1, 2, 3) Chiều dài dao Véc tơ đơn vị véc tơ Ai H i (i=1, 2, 3) Véc tơ đơn vị trục Ox, Oy, Oz Vận tốc góc cánh tay AiBi, BiCi (i=1, 2, 3) Véc tơ đơn vị vec tơ Ai Bi , Bi C i Vận tốc trọng tâm cánh tay AiBi, BiCi (i=1, 2, 3) Gia tốc góc cánh tay AiBi, BiCi (i=1, 2, 3) Gia tốc trọng tâm cánh tay AiBi, BiCi (i=1, 2, 3) Véc tơ gia tốc trọng trường vi DANH MỤC CÁC HÌNH VẼ VÀ ĐỒ THỊ Hình 1.1 Các mẫu máy CNC trục có cấu trúc kiểu PKM [7] Hình 1.2 Robot Line-Placer hãng Demaurex dùng để đóng gói bánh quy [20] Hình 1.3 SurgiScope vận hành thư viện Surgical Robotics [20] Hình 1.4 Robot IRB 340 FlexPicker hãng ABB Flexible Automation [20] Hình 1.5 Robot song song bậc tự Hexaglide, Đức [14] Hình 1.6 Máy phay 3D- CCCNC [15] Hình 1.7 Máy CNC trục IRCCyN [2] Hình 1.8 Mô hình phát triển IRCCyN [21] Hình 1.9 Tổng quan mô hình phát triển IRCCyN [21] Hình 1.10 Cấu trúc trực giao Orientation C-P-U PKM [15] Hình 1.11 Robot DOF ISOGLIDE3 [18] Hình 1.12 Máy song song-DOF [10] Hình 1.13 Robot trực giao PMK đại học California Hình 1.14 Cấu trúc 3-7R trực giao-1 [16] Hình 1.15 Cấu trúc 3-7R trực giao-2 [16] 10 Hình 1.16 Cấu trúc 3-DOF trực giao 10 Hình 1.17 Cấu trúc máy IRCCyN [2] 10 Hình 1.18 Tính đường dụng cụ với dao phay ngón đầu cầu 12 Hình 1.19 Tính đường dụng cụ với dao phay ngón đầu 13 Hình 1.20 Đường dụng cụ gia công tinh với dao đầu xuyến 14 Hình 1.21 Đường dụng cụ với dao phay ngón đầu cầu 15 Hình 1.22 Kết cấu máy nghiên cứu 16 Hình 2.1 Mô tả cấu trúc máy 17 Hình 2.2 Sơ đồ động học máy 18 Hình 2.3 Hệ tọa độ động cụm trục 19 Hình 2.4 Xét mặt YOZ cụm tay 22 Hình 2.5 Xét mặt XOZ cụm tay 23 Hình 2.6 Xét mặt XOY cụm tay 25 Hình 2.7 Sơ đồ tổng quát máy 27 Hình 2.8 Xét Jacobean cụm tay 29 Hình 2.9 Xét Jacobean cụm tay 31 Hình 2.10 Xét Jacobean cụm tay 33 Hình 2.11 Các bước dựng 2D 36 Hình 2.12 Khối solid sau extrude 37 Hình 2.13 Khối solid sau dựng cụm 37 Hình 2.14 Miền làm việc xây dựng phương pháp hình học 38 Hình 2.15 Thuật toán xác định miền làm việc 43 Hình 2.16 Minh họa thuật toán tìm miền bao 44 Hình 2.17 Thuật toán tìm miền bao 45 Hình 2.19 Miền làm việc máy 46 Hình 2.20 Biên dạng miền làm việc z=-445mm (trái) z=-334mm (phải) 46 Hình 2.21 Giao diện phần mềm 48 Hình 2.22 Load file liệu đầu vào 49 Hình 2.23 Chọn gốc máy 50 Hình 2.24 Chọn gốc phôi 50 vii Hình 2.25 Cài đặt dao đầu cầu 51 Hình 2.26 Cài đặt dao đầu phẳng 51 Hình 2.27 Cài đặt dao đầu xuyến 52 Hình 2.28 Kết mô 52 Hình 2.29 Phân tích tính vận tốc trọng tâm 55 Hình 2.30 Quỹ đạo mô vận tốc gia tốc 57 Hình 2.31 Đồ thị vận tốc, gia tốc góc AB 57 Hình 2.32 Đồ thị vận tốc, gia tốc góc BC 58 Hình 2.33 Đồ thị vận tốc trọng tâm AB 58 Hình 2.34 Đồ thị vận tốc trọng tâm BC 59 Hình 2.35 Đồ thị gia tốc trọng tâm AB 59 Hình 2.36 Đồ thị gia tốc trọng tâm BC 60 Hình 3.1 Sơ đồ tính toán động lực học cụm tay 63 Hình 3.2 Sơ đồ tính toán động lực học cụm tay 63 Hình 3.3 Sơ đồ tính toán động lực học cụm tay 64 Hình 3.4 Các lực cắt thành phần tác dụng lên dụng cụ 71 Hình 3.5 Chiếu lực cắt thành phần lên trục X, Y, Z 72 Hình 3.6 Lực cắt mô (nét đứt) lực cắt đo đạc trích dẫn từ [9] (nét liền) 73 Hình 3.7 Mô tả quỹ đạo mô 75 Hình 3.8 Quỹ đạo gia công 76 Hình 3.9 Đồ thị vận tốc góc, gia tốc góc cánh tay AiBi 76 Hình 3.10 Đồ thị vận tốc, gia tốc góc cánh tay BiCi 77 Hình 3.11 Lực truyền động gia công 77 Hình 4.1 Cấu thành điều khiển CNC 78 Hình 4.2 Cơ nội suy 82 Hình 4.3 Nội suy tuyến tính đoạn thẳng phương pháp hàm đánh giá 83 Hình 4.4 Nội suy tuyến tính đoạn cong 85 Hình 4.5 Thuật toán nội suy tuyến tính đoạn thẳng 86 Hình 4.6 Thuật toán nội suy tuyến tính cung tròn 87 Hình 4.7 Mô thuật toán nội suy đường thẳng 88 Hình 4.8 Mô thuật toán nội suy đường thẳng 88 Hình 4.9 Mô phòng nội suy mã Gcode 89 Hình 4.10 Nội suy đường thằng 89 Hình 4.11 Nội suy cung tròn G02 theo chiều thuận chiều kim đồng hồ 90 Hình 4.12 Nội suy cung tròn G03 theo chiều ngược chiều kim đồng hồ 90 Hình 4.13 Mô hình thuật toán điều khiển PD 92 Hình 4.14 Mô hình điều khiển 95 Hình 4.15 Khối cài đặt thông số PD 95 Hình 4.16 Luật điều khiển PD 95 Hình 4.17 Sai số vị trí với ĐK PD thông thường 97 Hình 4.18 Sai số vận tốc với ĐK PD thông thường 97 Hình 4.19 Lực truyền động theo điều khiển PD thông thường 97 Hình 4.20 Sai số vị trí với điều khiển PD 98 Hình 4.21 Sai số vận tốc với điều khiển PD 98 Hình 4.22 Lực truyền động theo điều khiển PD 98 Hình 4.23 Sai số vị trí với điều khiển PD 99 Hình 4.24 Sai só vận tốc với điều khển PD3 99 viii Hình 4.25 Lực truyền động theo điều khiển PD 99 Hình D.1 Thông số động lực học cụm cẳng tay 111 Hình D.2 Thông số động lực học cụm cánh tay 111 Hình D.3 Thông số động lực học cụm trượt 112 Hình D.4 Thông số động lực học cụm trục 112 ix DANH MỤC CÁC BẢNG Bảng 1.1 Trích ngang sở liệu 15 Bảng 2.1 Bảng thông số động học 35 Bảng 2.2 Cấu trúc file liệu 42 Bảng 3.1 Kích thước hình học loại dụng cụ mô 73 Bảng 3.2 Các hệ số lực cắt xác định cho dụng cụ phay đầu [9] 73 Bảng 3.3 Các giá trị lực cắt lớn mô thí nghiệm [9] 73 Bảng 3.4 Bảng thông số động lực học 74 Bảng 3.5 Bảng trích ngang quỹ đạo mô lực 75 Bảng 4.3 Kết mô (n=51) 96 x Dựa vào kết mô đồ ta có vài nhận xét sau: - Với điều khiển đầu tiên, PD thông thường, ta thấy, sai số vị trí nhỏ 6x10-3 m sau 0.5 s, sai số bình phương trung bình 0,0062 - Bộ điều khiển thứ 2, PD có bù trọng trường, ta thấy cải thiện đáng kể khả bám quỹ đạo, sau 0.5 s sai số vị trí nhỏ 1x10-3 m với sai số bình phương trung bình 0.0011 - Ở điều khiển cuối cùng, PD với đầy đủ thông số động lực học, ta thấy sai số bình phương trung bình vị trí nhỏ phương pháp 8,5012x10-4 sai số vị trí nhỏ 0.5x10-4 sau 0.5 s Bộ điều khiển cho thấy khả bám quỹ đạo tốt thực tế ta nên sử dụng điều khiển 100 KẾT LUẬN I KẾT QUẢ ĐẠT ĐƯỢC CỦA LUẬN VĂN Luận văn trình bày 101 trang A4 với kết đạt sau:  Về mặt lý thuyết Luận văn giải vấn đề sau: - Bài toán động học bao gồm (Bài toán vị trí, miền làm việc, vận tốc, gia tốc) - Bài toán động lực học điều khiển có xét đến yếu tố công nghệ - Xây dựng thuật toán nội suy điều khiển cốt lõi NCK điều khiển CNC - Xây dựng mô hình toán cho điều khiển phần cứng nhằm đáp ứng nội suy  Về mặt thực tế - Viết môđun phần mềm mô trình gia công nội suy theo G-code - Mô điều khiển Simulink Matlab II HƯỚNG PHÁT TRIỂN Luận văn dừng lại nghiên cứu lý thuyết, toán điều khiển chưa nghiên cứu sâu rộng, thiết lập điều khiển điều kiện lý tưởng bỏ qua yếu tố tác động bên ngoài: ma sát, rung động…, vấn đề nghiên cứu tiếp tương lai Ngoài ra, nghiên cứu tiếp theo, nghiên cứu lý thuyết thiết kế sơ phát triển, xây dựng mô hình thực tế ứng dụng gia công khí 101 TÀI LIỆU THAM KHẢO Nguyễn Văn Khang; Động lực học hệ nhiều vật; Nhà xuất khoa học kỹ thuật, Hà nội, 2007 Anatoly Pashkevich, Philippe WENGER, Damien CHABLAT; Kinematic and stiffness analysis of the Orthoglide, a PKM with simple, regular workspace and homogeneous performances, IEEE International Conference On Robotics And Automation, Rome, Italie, Avril, 2007.” Ayssam Elkady, Galal Elkobrosy, Sarwat Hanna and Tarek Sobh (2008); Cartesian ParallelManipulator Modeling, Control and Simulation, University of Bridgeport Carricato and Parenti-Castelli, A Family of 3-DOF Translational Parallel Manipulators, Proceedings of the 2001 ASME Design Engineering Technical Conferences, Pittsburgh, PA, DAC-21035 Ceccarelli, M., (1997), A New D.O.F Spatial Parallel Mechanism, Mechanism and Machine Theory, Vol 32, No 8, pp 895-902 Clavel, R., (1988), Delta, A Fast Robot with Parallel Geometry, Proceedings of the 18th International Symposium on Industrial Robots, pp 91-100 Félix Majou, Philippe Wenger, Damien Chablat; The design of parallel kinematic machine tools using kinetostatic erformance criteria, France J.-P.Merlet (2006), Parallel Robot,Spinger Janez Gradisek, Martin Kalveram, Klaus Weinert (2004); Mechanistic identification of specific force coefficients for a general endmill, International Journal of Machine Tools & Manufacture 44, pp 401–414 10 Jee-Hwan Ryu (2008); Parallel Manipulators, New Developments First published April 2008, pp 178-227, 477-487 102 11 Gullayanon R., (2005); Motion Control of Degree-of-Freedom Direct-Drive Robot, A master thesis presented to the School of Electrical and Computer Engineering, Georgia Institute of Technology 12 Kim H.S., and Tsai L.W., (2002); Design optimization of a Cartesian parallel manipulator, epartment of Mechanical Engineering, Bourns College of Engineering, University of California 13 Lewis, F., Abdallah, C and Dawson, D., (1993); Control of Robot Manipulators, MacMillan Publishing Company 14 Marcel Honegger; Nonlinear adaptive control of a dof parallel manipulator, Institute of Robotics, ETH Zurich, Switzerland 15 Masimo Callegari, Alessandra Ssuardi; Functionally- Oriented PKMs for Robot Cooperation, Italy 2004 16 Ping-Lang Yen, Chi-Chung Lai (2009); Dynamic modeling and control of a 3-DOF Cartesian parallel manipulator, Mechatronics 19 (2009) 390– 398 17 Serafettin Engin, Yusuf Altintas; Generalized modeling of milling mechanics and dynamics, The University of Brittish Columbia 18 Sergiu-Dan Stan1, Milos Manic2, Vistrian Mătieş1, Radu Bălan1 (2008); Kinematics Analysis, Design, and Control of an Isoglide3 Parallel Robot (IG3PR) 19 Tsai, L W., and Joshi, S., (2002); Kinematic Analysis of 3-DOF Position Mechanism for Use in Hybrid Kinematic Machines, ASME Journal of Mechanical Design, Vol 124, No 2, pp 245-253 20 http://www.parallemic.org/Reviews/Review002.html 21 http://www.parallemic.org/Reviews/Review011.html 103 PHỤ LỤC A PHẦN MÔ PHỎNG LẬP TRÌNH GIA CÔNG  CHƯƠNG TRÌNH VẼ void CLAM_CDT2View::Paint() { m_QuyDao.addPoint(CPoint4D(x,y,z-128)); m_pInitGL->BeforeDrawing(); glPushMatrix(); m_KhungRoBot.setMaterial(storageMaterial[10]); m_ConTruot.setMaterial(storageMaterial[13]); m_Cum1.setMaterial(storageMaterial[9]); m_Cum2.setMaterial(storageMaterial[9]); m_CumDauDao.setMaterial(storageMaterial[13]); m_CumVM.setMaterial(storageMaterial[3]); m_pInitGL->UseUCS(); m_pInitGL->TranslateGL(); m_pInitGL->MouseMoveGL(); m_pInitGL->ScaleGL(); if(init) { glPushMatrix(); ToaDoDauDao(); m_KhungRoBot.drawObject(); m_CumVM.drawObject(); glTranslated(0,0,dz); m_ConTruot.drawObject(); glTranslated(0,51,27); glRotated((-180*theta1)/Pi,0,0,1); m_Cum1.drawObject(); glTranslated(0,200,0); glRotated((-180*beta1)/Pi-90,0,0,1); m_Cum2.drawObject(); glTranslated(220,0,0); glRotated(180,0,0,1); glRotated((180*alpha1)/Pi,0,0,1); m_CumDauDao.drawObject(); glPopMatrix(); glPushMatrix(); glTranslated(0,-139,526); glRotated(-90,0,1,0); glRotated(90,0,0,1); glTranslated(0,0,dx); 104 m_ConTruot.drawObject(); glTranslated(0,51,27); glRotated((-180*theta2)/Pi,0,0,1); m_Cum1.drawObject(); glTranslated(0,200,0); glRotated(-90+(180*beta2)/Pi,0,0,1); m_Cum2.drawObject(); glPopMatrix(); glPushMatrix(); glTranslated(-619.6,0,496); glRotated(-90,1,0,0); glTranslated(0,0,dy); m_ConTruot.drawObject(); glTranslated(0,51,27); glRotated((180*theta3)/Pi,0,0,1); m_Cum1.drawObject(); glTranslated(0,200,0); glRotated(-90-(180*beta3)/Pi,0,0,1); m_Cum2.drawObject(); glPopMatrix(); m_QuyDao.drawPoints(); if(m1_phoi) ToaDoPhoi(); glPopMatrix(); if(m1_bemat) DrawFace(); } m_pInitGL->AfterDrawing(); }  HÀM TÍNH TOÁN ĐỘNG HỌC void CCTRPANEL::Calculate() { UpdateData(true); double Pi=3.145926535897932384626433832795; double l1,l2,a1,b1,l3,a2; double l,h,k,m,k3,h3; x1= m_workPieceZero.m_xgocphoi+a[t]*2; y1= m_workPieceZero.m_ygocphoi+b[t]*2; z1= m_workPieceZero.m_zgocphoi+c[t]*2; l1=200;l2=220;a1=0;b1=87;l=51;a2=87;k=526,h=496,m=6 19.6; m_dz=z1-27; m_dx=-x1-27; m_dy=y1-27-a1; l3=sqrt((y1-a1-l)*(y1-a1-l)+(x1+b1)*(x1+b1)); m_beta1=acos((l1*l1+l2*l2-l3*l3)/(2*l1*l2)); 105 m_theta1=acos((l1*l1+l3*l3-l2*l2)/(2*l1*l3))acos((y1-a1-l)/l3); m_alpha1=-acos((l2*l2+l3*l3l1*l1)/(2*l2*l3))+asin((y1-a1-l)/l3); k3=sqrt((y1-a2+139)*(y1-a2+139)+(k-l-z1)*(k-l-z1)); m_theta2=-acos((l1*l1+k3*k3l2*l2)/(2*l1*k3))+asin((y1-a2+139)/k3); m_beta2=acos((l1*l1+l2*l2-k3*k3)/(2*l1*l2)); h3=sqrt((m+x1-b1)*(m+x1-b1)+(h-z1-l)*(h-z1-l)); m_beta3=-acos((l1*l1+l2*l2-h3*h3)/(2*l1*l2)); m_theta3=acos((l1*l1+h3*h3-l2*l2)/(2*l1*h3))asin((m+x1-b1)/h3); m_xp1=x1; m_yp1=y1; m_zp1=z1; UpdateData(false); } B CHƯƠNG TRÌNH XÁC ĐỊNH MIỀN LÀM VIỆC clear; % Nhap cac thoong so kich thuoc may % l1=200;l2=220;h=620; a=87;b=87;a1=115;a2=145;a3=117; d1min=256;d1max=526; d2min=246;d2max=516; d3min=-415;d3max=-145; % tinh vi phan tren cac truc cua may % dd1=(d1max-d1min)/20; dd2=(d2max-d2min)/20; dd3=(d3max-d3min)/20; [FileName,PathName] = uiputfile('*.txt','Save Result As'); out = fopen([PathName,FileName],'w+'); x=[];y=[];z=[];x(1)=d1min;y(1)=d2min;z(1)=d3min;m=1; for k=1:20 z(k+1)=z(k)+dd3; for j=1:20 y(j+1)=y(j)+dd2; for i=1:20 x(i+1)=x(i)+dd1; if (((x(i)+b-h)^2+(y(j)a3)^2)(l1l2)^2)&&(((x(i)-b)^2+(z(k)+a2)^2)(l1-l2)^2)&&(((z(k)+a1)^2+(y(j)a)^2)(l1-l2)^2) xp(m)=x(i);yp(m)=y(j);zp(m)=z(k); xp1(m)=x(i);yp1(m)=y(j);zp1(m)=z(k)+0.3; fprintf(out,'%f %f %f\n',xp(m),yp(m),zp(m)); m=m+1; end; end; end; end; fclose(out); quiver3(xp,yp,zp,zp1,yp1,zp1,'LineWidth',2); C THUẬT TOÁN NỘI SUY GCODE  Nội suy tuyến tính đoạn thẳng phương pháp hàm đánh giá Xsl=[0 50];Ysl=[0 50];dx=3;dy=3; X=[];Y=[];F=[];X(1)=Xsl(1);Y(1)=Ysl(1); %tinh toan gia so i=1; F(1)=Ysl(1)*dx-Xsl(1)*dy; while ((abs(X(i)-Xsl(2))>dx)|(abs(Y(i)-Ysl(2))>dy)) if (F(i)==0|F(i)>0) F(i+1)=(Y(i)*dx-X(i)*dy)-dy; X(i+1)=X(i)+dx; Y(i+1)=Y(i); i=i+1; end if F(i)dx)|(abs(Y(i)-Ysl(2))>dy)) if (F(i)==0|F(i)>0) F(i+1)=((X(i)-1)^2)+(Y(i)^2)-(R^2); X(i+1)=X(i)-dx; Y(i+1)=Y(i); i=i+1; end if F(i)dx)|(abs(Y(i)-Ydl(2))>dy)) hx=kc1((X(i)+dx),Y(i),Xdl(1),Ydl(1),a,b); hy=kc1(X(i),(Y(i)+dy),Xdl(1),Ydl(1),a,b); if (hxdy)|(abs(z-zb)>dz)) hx=kc((x+dx),y,z,xa,ya,za,chx,chy,chz); hy=kc(x,(y+dy),z,xa,ya,za,chx,chy,chz); hz=kc(x,y,(z+dz),xa,ya,za,chx,chy,chz); if (hx