(Đồ án hcmute) thiết kế và chế tạo cánh tay robot bằng công nghệ in 3d

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 CÔNG NGHỆ KỸ THUẬT ÐIỆN TỬ TRUYỀN THÔNG THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT BẰNG CÔNG NGHỆ IN 3D GVHD: NGUYỄN THANH TÂM SVTH : TRẦN KHÁNH KHOA MSSV: 14141156 SKL 0 4 Tp Hồ Chí Minh, tháng 01/2019 an BỘ GIÁO DỤC & ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH - ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ TRUYỀN THÔNG ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT BẰNG CÔNG NGHỆ IN 3D GVHD: THS NGUYỄN THANH TÂM SVTH : TRẦN KHÁNH KHOA MSSV : 14141156 Tp Hồ Chí Minh - 01/2019 an TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH o0o -Tp HCM, ngày 07 tháng 01 năm 2019 NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: Trần Khánh Khoa MSSV: 14141156 Chuyên ngành: Điện tử Công ghiệp Mã ngành: 41 Hệ đào tạo: Đại học quy Mã hệ: 14941 Khóa: 2014 Lớp: 14941DT I TÊN ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT BẰNG CÔNG NGHỆ IN 3D II NHIỆM VỤ Các số liệu ban đầu: - Chế tạo cánh tay robot trục sử dụng động bước - Giao tiếp phần mềm máy tính cánh tay robot tập lệnh gcode Nội dung thực hiện: - NỘI DỤNG 1: In 3D lắp ráp cánh tay robot bậc sử dụng động bước - NỘI DUNG 2: Thiết kế thi công mạch điều khiển robot qua driver động bước - NỘI DUNG 3: Tìm hiểu tập lệnh Gcode CNC cơng nghiệp - NỘI DUNG 4: Viết chương trình cho vi điều khiển điều khiển cánh tay robot qua tâp lệnh gcode - NỘI DUNG 5: Giao tiếp phần mềm máy tính, điều khiển cánh tay robot thơng qua tập lệnh Gcode - NỘI DUNG 6: Khảo sát phần mềm xuất Gcode điều khiển cánh tay robot - NỘI DUNG 7: Khảo sát ứng dụng cánh tay robot điều khiển tập lệnh Gcode: vẽ 2D, in 3D, khắc laser, khoan CNC,… - NỘI DUNG 8: Lắp ráp chạy thử nghiệm hệ thống ii an - NỘI DUNG 9: Cải tiến mơ hình sản phẩm - NỘI DUNG 10: Viết sách luận văn - NỘI DUNG 11: Phản biện bổ sung, chỉnh sửa luận văn - NỘI DUNG 12: Báo cáo đề tài tốt nghiệp III NGÀY GIAO NHIỆM VỤ: 15/10/2018 IV NGÀY HOÀN THÀNH NHIỆM VỤ: 15/01/2019 V HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN: CÁN BỘ HƯỚNG DẪN ThS Nguyễn Thanh Tâm BM ĐIỆN TỬ CÔNG NGHIỆP – Y SINH iii an TRƯỜNG ĐH SPKT TP HỒ CHÍ MINH KHOA ĐIỆN-ĐIỆN TỬ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP – Y SINH CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM ĐỘC LẬP - TỰ DO - HẠNH PHÚC o0o -Tp HCM, ngày tháng 10 năm 2018 LỊCH TRÌNH THỰC HIỆN ĐỒ ÁN TỐT NGHIỆP Họ tên sinh viên: TRẦN KHÁNH KHOA Lớp: 14941DT MSSV: 14141156 Tên đề tài: THIẾT KẾ VÀ CHẾ TẠO CÁNH TAY ROBOT BẰNG CÔNG NGHỆ IN 3D Xác nhận Tuần/ngày Nội dung Tuần 03/10/2018 – 07/10/2018 Tuần 08/10/2018- 14/10/2018 Tuần 15/10/2018 – 21/10/2018 Tuần 22/10/2018 – 28/10/2018 - Gặp GVHD để nghe phổ biến yêu cầu làm đồ án, tiến hành chọn đồ án Tuần 5, 29/10/2018 – 11/11/2018 Tuần 7, 12/11/2018 – 25/11/2018 Tuần 9, 10, 11 26/11/2018 – 16/12/2018 Tuần 12-13 17/12/2018 – 30/12/2018 Tuần 14 31/12/2018 – 6/01/2019 Tuần 15 7/01/2019- 13/01/2019 GVHD - GVHD tiến hành xét duyệt đề tài - Viết đề cương tóm tắt nội dung đồ án - Tiến hành thiết kế sơ đồ khối, giải thích chức khối - Lựa chọn linh kiện cho khối - Thiết kế sơ đồ nguyên lý giải thích hoạt động mạch - Lựa chọn mua linh kiện Kiểm tra linh kiện - Tiến hành thi công mạch - Kiểm tra mạch thi công - Kiểm tra mạch thi cơng - Viết chương trình điều khiển thử nghiệm kiểm tra hoạt động mạch điều khiển - Chạy thử nghiệm hệ thống tối ưu chương trình - Viết báo cáo nội dung làm - Hoàn thiện báo cáo gởi cho GVHD để xem xét góp ý lần cuối trước in báo cáo - Làm slide thuyết trình chuẩn bị cho bảo vệ đồ án tốt nghiệp GV HƯỚNG DẪN iv an LỜI CAM ĐOAN Tôi – Trần Khánh Khoa cam đoan đồ án tốt nghiệp cơng trình nghiên cứu thân hướng dẫn thầy Th.S Nguyễn Thanh Tâm Các kết công bố đồ án tốt nghiệp trung thực không chép từ cơng trình khác Người thực đề tài Trần Khánh Khoa v an LỜI CẢM ƠN Để hồn thành đề tài này, em thực xin gửi lời cảm ơn chân thành đến Thầy/Cô khoa Điện - Điện Tử, trường Đại học Sư Phạm Kỹ Thuật Thành Phố Hồ Chí Minh, người truyền đạt cho em kiến thức quý báu, dẫn định hướng cho em trình học tập Đây tiền đề để em hồn thành đề tài nghiệp sau Đặc biệt, em xin chân thành cảm ơn Thầy Nguyễn Thanh Tâm tận tình hướng dẫn tạo điều kiện thuận lợi cho em suốt thời gian thực Đồ Án Tốt Nghiệp Em xin phép gửi đến thầy lòng biết ơn, lời cảm ơn chân thành sâu sắc Kiến thức, kinh nghiệm tâm nghề nghiệp thầy khơng giúp đỡ nhóm hồn thành tốt đề tài mà cịn gương để nhóm học tập noi theo đường sau Cuối cùng, mặc dù cố gắng hoàn thành nhiệm vụ đề tài đặt đảm bảo thời hạn kiến thức cịn non yếu nên chắn khơng tránh khỏi thiếu sót, mong q Thầy/Cơ bạn sinh viên thông cảm Rất mong nhận ý kiến Thầy/Cô bạn sinh viên Em xin chân thành cảm ơn! vi an MỤC LỤC Trang bìa…………… ………………………………………………………………i Nhiệm vụ đồ án tốt nghiệp ii Lịch trình thực đồ án tốt nghiệp iv Lời cam đoan v Lời cảm ơn vi Mục lục vii Liệt kê hình vẽ x Liệt kê bảng vẽ xii Tóm tắt xiii Chương TỔNG QUAN 1.1 Đặt vấn đề 1.2 Mục tiêu nghiên cứu 1.2.1 Mục tiêu nghiên cứu 1.2.2 Phương pháp nghiên cứu 1.3 Nội dung nghiên cứu 1.4 Giới hạn 1.5 Bố cục Chương CƠ SỞ LÝ THUYẾT 2.1 Tổng quan robot 2.1.1 Quá trình phát triển 2.1.2 Ứng dụng Robot 2.1.3 Phân loại Robot 2.2 Tổng quan robot công nghiệp 12 2.2.1 Robot công nghiệp 12 2.2.2 Cấu trúc Robot công nghiệp 13 2.2.3 Kết cấu tay máy 14 2.3 Ngôn ngữ gcode ứng dụng cnc 17 2.3.1 Khái niệm Gcode 17 2.3.2 Các lệnh Gcode lập trình CNC 17 2.4 Công nghệ in 3d 25 vii an 2.5 Động học nghịch cánh tay robot bậc 27 2.6 Chuẩn truyền liệu nối tiếp – uart 30 2.6.1 Khái niệm UART .30 2.6.2 Các thông số .30 2.7 Giới thiệu phần cứng 31 2.7.1 Driver A4988 động bước .31 2.7.2 Động bước 32 2.7.3 Arduino Uno R3 .32 2.7.4 Board CNC Shield V3 34 2.7.5 Cơng tắc hành trình 34 Chương TÍNH TỐN VÀ THIẾT KẾ 35 3.1 Giới thiệu 35 3.2 Tính tốn thiết kế hệ thống 35 3.2.1 Sơ đồ khối hệ thống 35 3.2.2 Tính tốn thiết kế mạch .36 Chương THI CÔNG HỆ THỐNG 49 4.1 Giới thiệu 49 4.2 Thi công phần cứng 49 4.2.1 Thi công cánh tay robot 49 4.2.2 Thi công mạch điều khiển 53 4.3 Lập trình hệ thống 54 4.3.1 Lưu đồ giải thuật 54 4.3.2 Phần mềm Arduino IDE 58 4.3.3 Phần mềm Inkscape tạo file gcode 60 4.3.4 Phần mềm gcode sender điều khiển cánh tay robot 61 4.4 Sử dụng gcode sender google chrome điều khiển cánh tay robot 62 Chương KẾT QUẢ, NHẬN XÉT VÀ ĐÁNH GIÁ 62 5.1 Kết 62 5.2 Nhận xét đánh giá 62 5.3.1 Ưu điểm hệ thống 62 5.3.2 Nhược điểm hệ thống .63 Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 64 viii an 6.1 Kết luận 64 6.2 Hướng phát triển 64 TÀI LIỆU THAM KHẢO 66 PHỤ LỤC 67 ix an CHƯƠNG THI CÔNG HỆ THỐNG - Load file gcode thực thi tập lệnh file phần mềm Phần di chuyển cánh tay tọa độ Oxy hiển thị phần mềm Hình 4.20: Đọc file thực thi lệnh gcode gcode sender BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 64 CHƯƠNG KẾT QUẢ, NHẬN XÉT VÀ ĐÁNH GIÁ Chương KẾT QUẢ, NHẬN XÉT VÀ ĐÁNH GIÁ 5.1 KẾT QUẢ Sau khoảng thời gian thực đề tài, nhóm chúng em đạt số kết sau: - Lắp ráp phát triển thành cơng mơ hình cánh tay robot trục điều khiển thông qua ngôn ngữ gcode CNC - Khảo sát tính tốn động học nghịch cánh tay robot bậc - Tìm hiểu ứng dụng ngôn ngữ Gcode ứng dụng CNC - Tìm hiểu sử dụng KIT Arduino Uno R3 ứng dụng CNC - Sử dụng CNC shield driver A4988 điều khiển động bước - Kết hợp công tắc hành trình xác định vị trí tọa độ gốc cánh tay robot - Khảo sát sử dụng phần mềm gcode sender điều khiển cánh tay robot - Khả sáng tạo lắp ráp phần khí hệ thống cánh tay robot - Lập trình cho cánh tay robot theo mong muốn - Kỹ tìm kiếm tài liệu, chọn lọc tài liệu nâng cao, tham khảo tài liệu tiếng anh 5.2 NHẬN XÉT VÀ ĐÁNH GIÁ Cánh tay robot nhận lệnh điều khiển từ người dùng để thực công việc lập trình tước Hoạt động đạt yêu cầu, khả nhận lệnh Gcode từ máy tính thực thao tác dịch chuyển theo yêu cầu 5.3.1 Ưu điểm hệ thống Hệ thống hoạt động đạt yêu cầu có ưu điểm: - Điều khiển tọa độ cánh tay xác khơng gian chiều - Phần cứng điều khiển gọn nhẹ, dễ dàng lắp đặt thay - Phần cứng cánh tay robot có độ thẩm mỉ cao, hoạt động tương đối êm - Các nét vẽ theo đường thẳng, đường cong xác - Hệ thống hoạt động ổn định, dễ dàng điều khiển với phần mềm gcode sender google chrome app - Khối nguồn, khối điều khiển, driver động bước động khơng bị nóng q trình hoạt động số loại máy CNC thị trường BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 62 CHƯƠNG KẾT QUẢ, NHẬN XÉT VÀ ĐÁNH GIÁ 5.3.2 Nhược điểm hệ thống Bên cạnh ưu điểm, hệ thống hạn chế cần khắc phục: - Phần cứng chịu lực thấp nên chưa thể tích hợp tool có trọng lượng lớn phần đầu cánh tay - Cánh tay robot có vùng phạm vi hoạt động nhỏ - Chưa có phần mềm đồng hồn toàn với hệ thống, phải sử dụng phần mềm thay mã nguồn mở BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 63 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 KẾT LUẬN Sau thời gian tìm hiểu thực đề tài, nhóm chúng em thu thập lượng kiến thức, kinh nghiệm từ việc nghiên cứu, thiết kế, chế tạo mạch đến việc lắp ráp, thi công hệ thống biết cách vận dụng kiến thức học vào thực tế Mơ hình cánh tay robot hoạt động đạt yêu cầu đặt như: - Khảo sát tính tốn xác động học nghịch cánh tay robot bậc - Khả nhận liệu điều khiển thực điều khiển hệ thống hoạt động xác - Dữ liệu tiếp nhận qua phần mềm gcode sender truyền đến vi điều khiển xác để vi điều khiển thực cơng việc - Mơ hình cánh tay robot nhận lệnh gcode điều khiển di chuyển xác đến vị trí mong muốn khơng gian chiều - Hệ thống endstop với cơng tác hành trình hoạt động tốt, xác định vị trí gốc làm việc cánh tay - Phần mềm gcode sender điều khiển mơ hình cánh tay robot theo chế dộ: nút nhấn điều khiển, đọc lệnh qua tập tin gcode 6.2 HƯỚNG PHÁT TRIỂN Vì thời gian thực đề tài có giới hạn nên cịn nhiều phương án - thiết kế để thay đổi - bổ sung thêm cho hệ thống mà nhóm chưa thể tìm hiểu thực Những phương án bổ sung nhằm tăng thêm chức năng, mở rộng khả điều khiển, tính linh hoạt cho hệ thống Cụ thể hướng phát triển mà nhóm nghĩ đến sau: - Sử dụng mơ hình cánh tay robot dạng hình học khác chắn sai số thấp mô hình sử dụng - Sử dụng động bước có tích hợp encoder để xác định góc quay có độ xác - Tích hợp thêm cơng cụ vào đầu cánh tay như: đầu in 3D, đầu khắc laser, đầu khoan CNC, đầu kẹp,… để mở rộng ứng dụng cánh tay robot BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 64 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN - Tăng số trục robot lên nhằm tăng phạm vi hoạt động tăng tính linh hoạt cho robot - Viết thêm phần mềm điều khiển máy tính đồng với mơ hình robot: tích hợp thêm hình ảnh 3D mơ chuyển động cánh tay thực tế BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 65 TÀI LIỆU THAM KHẢO TÀI LIỆU THAM KHẢO Các tài liệu tham khảo: [1] Nguyễn Đình Phú, Phan Vân Hồn, Trương Ngọc Anh, “Giáo trình vi điều khiển PIC”, NXB ĐH Quốc gia Tp.HCM, 2018 [2] Bùi Quý Tuấn, "Programming manual for CNC mill", HaNoi University of Industry, 2017 [3] MachMotion, "Summary of Gcode commands", https://machmotion.com/cncinfo/g-code.html, 2018 [4] Nguyễn Văn Thái, "Động học nghịch tay máy", https://www.youtube.com/watch?v=p1wIJut1bTs, Digital learning center HCMC youtube channel, 2016 [5] Saha S.K, "Inverse Kinematics", IIT Delhi, 2009 [6] trieutuan_vnu, "Robot Arm MK2 Plus", https://www.instructables.com/id/Robot-Arm-MK2-Plus-Stepper-Motor-Used/, viewed Nov 2018 [7] Arduino.vn, “Hướng dẫn điều khiển động bước”, http://arduino.vn/baiviet/685-huong-dan-dieu-khien-dong-co-buoc, 2018 [8] Nguyễn Phát Lợi, “Cấu tạo động bước”, http://www.nguyenphatloi.com/2014/11/ong-co-buoc-step-motor.html, 2018 [9] Arduino.vn, “Điều khiển động bước pha”, http://arduino.vn/tutorial/1385-dieu-khien-module-dong-co-step-2-pha-va-thu-vien, 2018 Các datasheet tham khảo: Datasheet arduino Uno r3 Datasheet driver A4988 Datasheet CNC shield v3 Các trang chủ phần mềm sử dụng: Phần mềm arduino IDE: https://www.arduino.cc/ Phần mềm gcode sender: https://winder.github.io/ugs_website/ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 66 TÀI LIỆU THAM KHẢO Phần mềm gcode sender google chrome app: https://chrome.google.com/webstore/detail/gcodesender/ngncibnakmabjlfpadjagnbdjbhoelom Phần mềm inkscape: https://inkscape.org/ BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 67 PHỤ LỤC PHỤ LỤC Các define, số, biến số: /* -* Global defination * -*/ //define gpio pin #define STEP_X_AXIS //step pin - X #define STEP_Y_AXIS //step pin - Y #define STEP_Z_AXIS //step pin - Z #define DIRECTION_X_AXIS //direction pin - X #define DIRECTION_Y_AXIS //direction pin - Y #define DIRECTION_Z_AXIS //direction pin - Z #define ENABLE_PIN //enable pin #define LIMIT_X_AXIS //limit pin - X #define LIMIT_Y_AXIS 10 //limit pin - Y #define LIMIT_Z_AXIS 11 //limit pin - Z //define default degree #define DEFAULT_ANPHA_1 //anpha #define DEFAULT_ANPHA_2 109.1 //anpha #define DEFAULT_ANPHA_3 75.5 //anpha //define for cnc #define MAX_BUF 64 //max buffer string of uart #define STEPS_SIZE 1.8 //step size motor #define MIN_FEEDRATE 1000 //max speed #define MAX_FEEDRATE 10 //min speed #define NUM_AXIES //number of axis #define CM_PER_SEGMENT //segment for arc #define TOOL_DISTANCE 3.51 #define RATIO_AXIS_3 2.474 #define RATIO_AXIS_2 2.474 #define RATIO_AXIS_1 5.684 #define CONVERT_INCH_TO_CM 2.54 #define MICRO_STEP 16 /* -* Global constant variable * */ const float L1 = 80; //value of L1 ARM const float L2 = 128; //value of L2 ARM const float L3 = 135; //value of L3 ARM const float CONVERT_RADIAN_TO_DEGREE = 57.29; //convert radian - degree const float CONVERT_DEGREE_TO_RADIAN = 0.017; //convert degree - radian /* -* Global variable * */ int mode_abs; //mode abs float a[3]; //current anpha array float newa[3]; //new anpha array float s[3]; //step array float sub[3]; //delta anpha char buffer[MAX_BUF]; //buffer of uart int sofar; //number of character float max_step; //max of step float step_delay = 1000;//default step delay float fr = 1000; //feedrate float Ax, Ay, Az; //current axis BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 67 PHỤ LỤC Hàm tính giá trị góc quay trục thứ nhất: static float getTheta1(float px, float py, float pz) { float Theta; Theta = atan2(py, px)*CONVERT_RADIAN_TO_DEGREE; return Theta; } Hàm tính giá trị góc quay thứ hai: static float getTheta2(float px, float py, float pz, float Deg) { float Anpha, Beta, Theta, SinAnpha, CosAnpha, SinBeta, CosBeta; SinAnpha = L3*sin(Deg*CONVERT_DEGREE_TO_RADIAN); CosAnpha = L2 - L3*cos(Deg*CONVERT_DEGREE_TO_RADIAN); Anpha = atan2(SinAnpha, CosAnpha); SinBeta = pz - L1; CosBeta = sqrt(px*px + py*py); Beta = atan2(SinBeta, CosBeta); Theta = (Anpha + Beta)*CONVERT_RADIAN_TO_DEGREE; return Theta; } Hàm tính giá trị góc quay thứ ba: static float getTheta3(float px, float py, float pz) { float CosTheta, SinTheta, Theta; CosTheta = (L2*L2 + L3*L3 - px*px - py*py - (pz - L1)*(pz L1))/(2*L2*L3); SinTheta = sqrt(1 - CosTheta*CosTheta); Theta = atan2(SinTheta, CosTheta)*CONVERT_RADIAN_TO_DEGREE; return Theta; } Chương trình tính số bước động cơ: void position(float px, float py, float pz) { int i; newa[0] = getTheta1(px, py, pz); newa[2] = getTheta3(px, py, pz); newa[1] = getTheta2(px, py, pz, newa[2]); newa[2] = newa[2] + newa[1]; for(i=0; i < NUM_AXIES; i++){ sub[i] = newa[i] - a[i]; } s[0] = MICRO_STEP*RATIO_AXIS_1*abs(sub[0])/STEPS_SIZE; s[1] = MICRO_STEP*RATIO_AXIS_2*abs(sub[1])/STEPS_SIZE; s[2] = MICRO_STEP*RATIO_AXIS_3*abs(sub[2])/STEPS_SIZE; } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 68 PHỤ LỤC Chương trình chọn chiều quay cho động cơ: void direction_motor() { if(sub[0] >= 0) digitalWrite(DIRECTION_X_AXIS, LOW); else digitalWrite(DIRECTION_X_AXIS, HIGH); if(sub[1] >= 0) digitalWrite(DIRECTION_Y_AXIS, LOW); else digitalWrite(DIRECTION_Y_AXIS, HIGH); if(sub[2] >= 0) digitalWrite(DIRECTION_Z_AXIS, LOW); else digitalWrite(DIRECTION_Z_AXIS, HIGH); } Chương trình xuất bước đến driver động cơ: void oneStep(int _motor) { if(_motor == 0) { digitalWrite(STEP_X_AXIS, digitalWrite(STEP_X_AXIS, } else if(_motor == 1) { digitalWrite(STEP_Y_AXIS, digitalWrite(STEP_Y_AXIS, } else if(_motor == 2) { digitalWrite(STEP_Z_AXIS, digitalWrite(STEP_Z_AXIS, } } HIGH); LOW); HIGH); LOW); HIGH); LOW); Chương trình di chuyển đến tọa độ void line(float px, float py, float pz) { int i, j; long over_1 = 0; long over_2 = 0; long over_3 = 0; position(px, py, pz); int is_max_step = 0; direction_motor(); max_step = s[0]; if(s[1] > max_step) { max_step = s[1]; is_max_step = 1; } if(s[2] > max_step) { max_step = s[2]; is_max_step = 2; } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 69 PHỤ LỤC //max step is step motor if(is_max_step == 0) { for(i=0; i < max_step; i++) { oneStep(0); over_1 = over_1 + s[1]; over_2 = over_2 + s[2]; if(over_1 >= s[0]) { over_1 = over_1 - s[0]; oneStep(1); } if(over_2 >= s[0]){ over_2 = over_2 - s[0]; oneStep(2); } pause(step_delay); } } //max step is step motor else if(is_max_step == 1){ for(i=0; i < max_step; i++) { oneStep(1); over_1 = over_1 + s[0]; over_2 = over_2 + s[2]; if(over_1 >= s[1]) { over_1 = over_1 - s[1]; oneStep(0); } if(over_2 >= s[1]){ over_2 = over_2 - s[1]; oneStep(2); } pause(step_delay); } } //max step is step motor else if(is_max_step == 2){ for(i=0; i < max_step; i++) { oneStep(2); over_1 = over_1 + s[0]; over_2 = over_2 + s[1]; if(over_1 >= s[2]) { over_1 = over_1 - s[2]; oneStep(0); } if(over_2 >= s[2]){ over_2 = over_2 - s[2]; oneStep(1); } pause(step_delay); } } //save this position Ax = px; Ay = py; Az = pz; for(i=0; i < NUM_AXIES; i++){ a[i] = newa[i]; } BỘ MÔN ĐIỆN TỬ CÔNG NGHIỆP Y SINH an 70 PHỤ LỤC Chương trình vẽ cung tròn nhỏ 180 độ: void arc(float cx, float cy, float x, float y, float dir) { // get radius float dx = Ax - cx; float dy = Ay - cy; float radius = sqrt(dx*dx + dy*dy); // find angle of arc (sweep) float angle1 = atan3(dy, dx); float angle2 = atan3(y - cy, x - cx); float theta = angle2 - angle1; if(dir>0 && theta