Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta Đồ án thiết kế hệ thống thiết bị tự động CK-CĐT với đề tài thiết kế robot song song delta
MỤC LỤC CHƯƠNG 1: NHỮNG KIẾN THỨC CƠ SỞ VỀ ROBOT CÔNG NGHIỆP 1.1 - Lịch sử phát triển robot công nghiệp 1.2 - Định nghĩa phân loại 1.2.1 - Định nghĩa 1.2.2 - Phân loại robot 1.3 - Ứng dụng robot công nghiệp 11 1.4 - Giới thiệu dạng robot song song Robot Delta .13 1.4.1 - Robot có cấu trúc song song 13 1.4.2 - So sánh Robot nối tiếp robot song song 14 1.4.3 - Ứng dụng robot song song 17 1.5 - Tổng quan hệ thống thiết kế nhóm 20 CHƯƠNG 2: THIẾT KẾ HỆ THỐNG CƠ KHÍ VÀ XÂY DỰNG MƠ HÌNH ROBOT DELTA 24 2.1 - Phân tích cấu trúc 24 2.1.1 - Lựa chọn cấu trúc .24 2.1.2 - Cơ cấu dẫn động .25 2.1.3 - Hệ khớp dẫn động 32 2.1.4 - Hệ hình bình hành 35 2.1.5 - Giá động 36 2.1.6 - Gá cấu chấp hành 38 2.1.7 - Bàn tay kẹp .40 2.1.8 - Khung robot 41 2.1.9 - Các hệ thống phụ trợ 44 2.2 - Mơ hình tổng quan .47 2.3 - Thiết kế hệ dẫn động cho robot 50 2.3.1 - Tính chọn động 51 CHƯƠNG 3: XÂY DỰNG MƠ HÌNH CƠ HỌC VÀ MƠ HÌNH TỐN HỌC CHO ROBOT SONG SONG DELTA 53 3.1 - Mơ hình động học robot song song Delta khơng gian .53 3.1.1 - Mơ hình động học robot song song Delta 54 3.1.2 - Động học thuận Robot 59 3.1.3 3.2 - Động học ngược Robot 61 - Động lực học Robot Delta 65 CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 73 4.1 - Cấu trúc hệ thống điều khiển 73 4.2 - Các thành phần hệ thống điều khiển .74 4.2.1 - Kit điều khiển Arduino Nano 74 4.2.2 - Driver A4988 76 4.2.3 - Cảm biến hồng ngoại 78 4.3 - Lập trình điều khiển .80 4.3.1 - Phần mềm lập trình điều khiển 82 4.3.2 - Code điều khiển 84 LỜI KẾT 85 TÀI LIỆU THAM KHẢO 87 PHỤ LỤC 88 DANH MỤC HÌNH V Hình 1.1- Robot bậc tự [*] Hình 1.2 - Robot song song [*] .7 Hình 1.3- Robot hàn chuỗi hở [*] Hình 1.4- Cấu tạo động bước(Step Motor) [*] Hình 1.5 - Động khí nén[*] 10 Hình 1.6 - Bộ truyền dây dai bánh [*] 11 Hình 1.7- Biểu đồ phân bố robot cơng nghiệp lĩnh vực [*] 12 Hình 1.8- Hệ thống robot nhà máy VinFast [*] 13 Hình 1.9- Cấu trúc robot song song .14 Hình 1.10- Robot Puma 15 Hình 1.11- Hai robot song song Delta 3RUS 3PUS 17 Hình 1.12- Cơ cấu song song Gough 17 Hình 1.13- Robot Delta dùng công nghiệp thực phẩm 18 Hình 1.14- Cơ cấu Robot song song Stewart 19 Hình 1.15- Sản phẩm hãng École Nationale d‟E1quitation 19 Hình 1.16- Bộ mơ Viện KAIST Motek 20 Hình 1.17- Các thành phần hệ thống robot Delta 20 Hình 2.1- Robot Delta bậc tự 24 Hình 2.2- Robot Delta bậc tự 24 Hình 2.3- Dẫn động trực tiếp từ động .26 Hình 2.4- Dẫn động gián tiếp qua hệ bánh 26 Hình 2.5- Bản vẽ khớp nối động 28 Hình 2.6- Bản vẽ truyền 29 Hình 2.7- Đồ thị phương trình đường cong mỏi 31 Hình 2.8- Hệ khớp dẫn động 33 Hình 2.9- Bản vẽ khớp nối cầu 35 Hình 2.10- Hệ hình bình hành .35 Hình 2.11- Ống nhơm làm hệ hình bình hành .36 Hình 2.12- Giá động 37 Hình 2.13- Bản vẽ động bước 37 Hình 2.14- Gá cấu chấp hành 38 Hình 2.15- Bản vẽ gá đỡ tay gắp 39 Hình 2.16- Bàn tay kẹp 40 Hình 2.17- Tay gắp Robot G4 Gripper 41 Hình 2.18- Bản vẽ gá .42 Hình 2.19- Bản vẽ nhơm định hình .43 Hình 2.20- Nhơm định hình 43 Hình 2.21- Bản vẽ gá .44 Hình 2.22- Bản vẽ băng tải 46 Hình 2.23- Băng tải mini .46 Hình 2.24- Hình chiếu đứng mơ hình .47 Hình 2.25- Hình chiếu khơng gian mơ hình 48 Hình 2.26- Bản vẽ tổng 49 Hình 2.27- Động NEMA17 51 Hình 2.28- Thơng số kích thước động NEMA17 .52 Hình 3.1- Robot song song Delta3RUS 54 Hình 3.2-Mơ hình động học hệ tọa độ Robot song song Delta3RUS .55 Hình 3.3- Ký hiệu tham số tính tốn .58 Hình 3.4- Hình chiếu lên mặt phẳng OXY 59 Hình 3.5- Kí hiệu tham số tính tốn 62 Hình 3.6- Hình chiếu lên mặt phẳng OXZ 63 Hình 3.7- Kí hiệu tọa độ tính tốn 64 Hình 4.1- Sơ đồ cấu trúc hệ thống điều khiển .73 Hình 4.2- vi điều khiển Atemega328P 75 Hình 4.3- vi điều khiển Atemega328P Pinout .75 Hình 4.4- Driver A4988 76 Hình 4.5- Bảng chế độ hoạt động A4988 77 Hình 4.6- Cảm biến hống ngoại 78 Hình 4.7- Sơ đồ cấu tạo cảm biến hống ngoại 78 Hình 4.8- Sơ đồ mạch nguyên lí hệ thống điều khiển .81 Hình 4.9- Sơ đồ thuật tốn điều khiển 82 Hình 4.10- Giao diện lập trình phần mềm Arduino IDE 84 MỞ ĐẦU Trong sống ngày nay, Robot mang tới cho sống người sống mới, cách trải nghiệm sống đơi cịn người bạn Những hãng Robot (RB) từ nước tiếng giới từ Đức, Nhật bản, Nga, Mỹ ngày khẳng định diện RB phần không thiếu sống tương lai phía trước Nó xuất tất lĩnh vực từ khoa học vĩ mô vi mô ngày đa dạng Trong khuôn khổ môn học Đồ án thiết kế hệ thống CK-CĐT thiết bị tự động với đề tài thiết kế robot song song Delta, chúng em tin tưởng với kết có từ việc tìm hiểu tính tốn tiểu luận bước đệm quan trọng cho việc phát triển nhiều ý tưởng tương lai tính tốn thiết kế loại Robot cơng nghiệp Với bố cục gồm: 1- Tổng quan Robot Phần nhìn sơ qua Robot bao gồm lịch sử phát triển, phân loại ứng dụng giúp hình dung tính quan trọng hữu dụng tới sống 2- Xây dựng mơ hình thực tế Bao gồm bước tính tốn xây dựng mơ hình học để kiểm chứng tính đắn q trình thiết kế cung cấp q trình để xác định cách sản phẩm Robot đưa vào ứng dụng sống 3- Giải toán động học động lực học ngược Bao gồm nêu lên yêu cầu ý nghĩa tốn ngược, qua đề xuất phương pháp giải bước thực 4- Xây dựng mơ hình điều khiển thực tế cho robot song song Delta Từ tính tốn chương chương 3, nhóm đề xuất mơ hình thiết kế hệ thống điều khiển cho robot song song Delta số phần mềm đồ họa mô Em xin gửi lời cảm ơn tới thầy Nguyễn Văn A, cảm ơn thầy đóng góp qua giảng hướng dẫn trình trao đổi buổi gặp mặt Những góp ý, sửa chữa thầy phần giúp em tự tin cách thức tiếp cận với công nghiệp có chuẩn bị em kiến thức em mang đến báo cáo sai sót chưa Em mong có bổ sung, sửa chữa Em chân thành cảm ơn chúc thầy sức khoẻ! CHƯƠNG 1: NHỮNG KIẾN THỨC CƠ SỞ VỀ ROBOT CÔNG NGHIỆP TỔNG QUAN VÀ GIỚI THIỆU VỀ ROBOT SONG SONG 1.1 - Lịch sử phát triển robot công nghiệp Ngay sau chiến tranh giới thứ 2, Hoa Kì xuất tay máy chấp hành điều khiển từ xa phịng thí nghiệm vật liệu phóng xạ Vào năm 50 kỉ 20, bên cạnh tay máy chấp hành khí đó, xuất tay máy chấp hành thủy lực điện tử Năm 1961, robot công nghiệp đưa vào sử dụng nhà máy Genaral Motor Trenton, New Jersey, Hoa Kì Năm 1967, Nhật Bản nhập robot công nghiệp từ cơng ty AMF Hoa Kì Đến năm 1990, có 40 cơng ty Nhật Bản đưa thị trường nhiều loại robot tiếng Từ năm 70, việc nghiên cứu nâng cao tính robot ý nhiều đến lắp đặt thêm cảm biến ngoại tín hiệu để nhận biết mơi trường làm việc Một lĩnh vực nhiều phịng thí nghiệm quan tâm robot tự hành Các nghiên cứu robot tự hành bắt chước hoạt động chân người, chân động vật Các loại robot chưa có ứng dụng nhiều công nghiệp, nhiên loại xe robot (robotcar) lại nhanh chóng đưa vào ứng dụng hệ thống sản xuất linh hoạt Từ năm 80, năm 90, áp dụng rộng rãi ứng dụng kĩ thuật vi xử lý công nghệ thông tin, số lượng robot công nghiệp gia tăng, giá thành giảm rõ rệt, tính có nhiều bước tiến vượt bậc Nhờ robot cơng nghiệp có chỗ đứng dây chuyền sản xuất tự động 1.2 - Định nghĩa phân loại 1.2.1- Định nghĩa Có nhiều định nghĩa robot tồn tại, tham khảo số định nghĩa sau: Định nghĩa theo từ điển New World College “Robot kết cấu khí có hình dạng bất kì, xây dựng để thực công việc tay người” Các định nghĩa sau bao gồm cánh tay khí, máy móc điều khiển số, máy móc di chuyển theo kiểu bước mơ hình dáng người Các robot công nghiệp ngày thực phần công việc người Các robot ban đầu thường gọi tay máy (Manipulator) Định nghĩa theo hiệp hội robot công nghiệp Nhật Bản Định nghĩa mang tính khái quát tất định nghĩa sử dụng Nó bao gồm tất cá thiết bị tay máy xem định nghĩa robot sau “Robot máy, cấu thường gồm số phận phân đoạn nối với phân đoạn khác khớp quay hay khớp trượt nhằm mục đích để gắp hay di chuyển đối tượng, thường có số bậc tự Nó điều khiển nguồn kích hoạt, hệ thống điện tử lập trình hay hệ thống logic đó” Định nghĩa theo tiêu chuẩn AFNOR (Pháp) “Robot cấu chuyển đội tự động chương trình hóa, lặp lại chương trình, tổng hợp chương trình đặt sườn trục tọa độ, có khả định vị, định hướng, di chuyển đối tượng vật chất (chi tiết, dụng cụ gá lắp.v.v.) theo hành trình thay đổi chương trình hóa nhằm thực nhiệm vụ công nghệ khác nhau.” Định nghĩa theo hiệp hội robot cơng nghiệp Hoa Kì “Robot tay máy có nhiều chức lập trình, thiết kế để di chuyển vật liệu, phần tử, linh kiện, dụng cụ thiết bị đặc biệt thông qua việc thay đổi chương trình hoạt động lập để thực nhiệm vụ khác nhau.” Định nghĩa theo hiệp hội robot Anh “Robot cơng nghiệp thiết bị lập trình lại, thiết kế để thực hai nhiệm vụ cầm nắm vận chuyển phần tử, linh kiện, dụng cụ công cụ chế tạo đặc biệt thông qua việc thay đổi chương trình hoạt động thiết lập để thực tác vụ gia công khác nhau.” Định nghĩa GOST (Nga) “Robot máy tự động liên kết tay máy cụm điều khiển chương trình hóa, thực chu trình cơng nghệ cách chủ động với điều khiển thay chức tương tự người.” Như vậy, qua định nghĩa trên, ta hiểu đơn giản robot công nghiệp loại thiết bị thiết kế chế tạo để phục vụ số hoạt động q trình sản xuất, thơng qua chương trình thiết lập sẵn, thay đổi tùy vào mục đích sử dụng 1.2.2- Phân loại robot Việc phân nhóm, phân loại robot dựa yếu tố kĩ thuật khác Dưới số cách phân loại chủ yếu: a- Phân loại theo số bậc tự Định nghĩa Bậc tự cấu số thông số độc lập cần cho trước để xác định hoàn toàn vị trí khâu cấu cấu hoạt động Điều có nghĩa thơng số độc lập quy luật cho trước để xác định quy luật chuyển động cấu Khâu có quy luật chuyển động cho trước gọi khâu dẫn, khâu dẫn nối với giá khớp loại khớp loại có thơng số xác định Hình 1.1- Robot bậc tự [*] Trong H1.1 mơ hình robot bậc tự gồm bậc tự tịnh tiến bậc tự quay Cơng thức tính số bậc tự cấu Đối với robot dạng chuỗi động hở robot sacra xét số bậc tự tính theo cơng thức sau: Tính năng: Module Thu Phát Hồng Ngoại V1 loại cảm biến thông dụng dùng nhiều hệ thống cửa tự động thông minh, cảm biến an toàn cổng tự động barrie tự động, cổng co giãn inox tự động cảm biến phát vật cản hồng ngoại hay cảm biến IR (IR detector) Ứng dụng: Cửa tự động thông minh, chống trộm, phát vật cản, đếm sản phẩm, đếm số lượng người, Thông số kỹ thuật: Module phát vật cản khoảng cách từ - 30cm Góc phát hiện: 35° Khi phát vật cản, tín hiệu đầu OUT mức thấp đèn led màu xanh sáng Có thể điều chỉnh khoảng cách biến trở Chỉnh chiết áp để tăng khoảng cách theo chiều kim đồng hồ, ngược lại để giảm khoảng cách Cổng OUT điều khiển trực tiếp Rơ le 5V cổng IO MCU Điện áp cung cấp: - 5V DC Dòng điện tiêu thụ: 23 mA (3,3V), 43 mA (5V) 79 Nguyên lý hoạt động: Module Thu Phát Hồng Ngoại V1 tích hợp phát hồng ngoại thu hồng ngoại Bộ phát hồng ngoại diode phát sáng (LED) phát tia hồng ngoại Do đó, chúng gọi IR LED Mặc dù LED IR trông giống đèn LED bình thường, xạ phát từ IR LED sóng hồng ngoại nên người khơng thể nhìn thấy mắt thường Bộ thu hồng ngoại gọi cảm biến hồng ngoại chúng phát tia từ phát hồng ngoại Bộ thu hồng ngoại có dạng photodiode phototransistors Photodiode hồng ngoại khác với điốt thơng thường chúng phát xạ hồng ngoại Khi led phát hồng ngoại phát xạ, đến vật thể số xạ phản xạ lại led thu hồng ngoại Dựa cường độ thu led thu hồng ngoại, đầu cảm biến xác định mức cao thấp 4.3 - Lập trình điều khiển 80 Hình 4.59- Sơ đồ mạch nguyên lí hệ thống điều khiển 81 Hình 4.60- Sơ đồ thuật tốn điều khiển 4.3.1- Phần mềm lập trình điều khiển Phần mềm Arduino IDE Arduino IDE phần mềm mã nguồn mở chủ yếu sử dụng để viết biên dịch mã vào module Arduino Đây phần mềm Arduino thức, giúp cho việc biên dịch mã trở nên dễ dàng Nó có phiên cho hệ điều hành MAC, Windows, Linux chạy tảng Java kèm với chức lệnh có sẵn đóng vai trị quan trọng để gỡ lỗi, chỉnh sửa biên dịch mã mơi trường Có nhiều module Arduino Arduino Uno, Arduino Mega, Arduino Leonardo, Arduino Micro nhiều module khác Trong phạm vi đồ án này, nhóm sử dụng module Arduino Nano Mỗi module chứa vi điều khiển bo 82 mạch lập trình chấp nhận thơng tin dạng mã Mã chính, gọi sketch, tạo tảng IDE tạo file Hex, sau chuyển tải lên điều khiển bo Môi trường IDE chủ yếu chứa hai phần bản: Trình chỉnh sửa Trình biên dịch, phần đầu sử dụng để viết mã yêu cầu phần sau sử dụng để biên dịch tải mã lên module Arduino Môi trường hỗ trợ ngôn ngữ C C ++ Khi người dùng viết mã biên dịch, IDE tạo file Hex cho mã File Hex file thập phân Hexa Arduino hiểu sau gửi đến bo mạch cáp USB Mỗi bo Arduino tích hợp vi điều khiển, vi điều khiển nhận file hex chạy theo mã viết Arduino IDE bao gồm phần khác Window bar Menu bar Phím tắt Text Editor Output Panel 83 Hình 4.61- Giao diện lập trình phần mềm Arduino IDE 4.3.2- Code điều khiển (Phụ lục 1) 84 LỜI KẾT Trên báo cáo đồ án tốt nghiệp Thiết kế, chế tạo robot song song Delta chúng em Đây đề tài có tính thực tế cao, thời đại cơng nghiệp hóa đại hóa đất nước, ngày phát triển với cạnh tranh không ngừng từ nước giới Đòi hỏi suất chất lượng phải cải thiện nhờ dây chuyền máy móc đại thay lao động thủ công người Như đồ án tốt nghiệp chúng em tìm hiểu cách xây dựng mơ hình robot tuần tra, từ tính tốn thiết kế hệ thống khí đến lập trình điều khiển hoạt động Cơng việc hồn thành bao gồm: - Tổng quan robot thông minh - Xây dựng kết cấu 2D cho robot song song Delta phần mềm AutoCad - Dựng mơ hình 3D Solidworks - Tính tốn thơng số khí - Tìm hiểu ngun lí hoạt động vận dụng số linh kiện điện tử - Xây dựng sơ đồ khối sơ đồ nguyên lí mạch điều khiển - Lập trình điều khiển robot song song Delta hoạt động Qua đề tài em biết cách vận dụng kiến thức chuyên môn đào tạo trường Đại học Điện lực Hà Nội thời gian qua vào với thực tế sống với công nghiệp Không qua đồ án chúng em học nhiều kĩ làm việc nhóm, giải vấn đề, tìm tài liệu, viết báo cáo… có ích cho sau Một lần chúng em xin chân thành cảm ơn hướng dẫn tận tình thầy … Cùng thầy mơn Cơ Điện Tử giúp chúng em hồn thành đề tài Do giới hạn thời gian kiến thức đồ án này, chúng em giải số vấn đề việc thiết kế chế tạo robot song song Delta Tuy nhiên, ngồi cịn nhiều vấn đề cần phải giải khắc phục để trở thành sản phẩm hoàn thiện áp dụng tốt, phục vụ tốt sống nói 85 chung ngành cơng nghiệp nước nhà nói riêng chúng em vui mừng mong muốn quý thầy cô bạn đóng góp ý kiến để đề tài hoàn thiện Chúng em xin chân thành cảm ơn! 86 TÀI LIỆU THAM KHẢO Sách giáo trình, giảng Nguyễn Trọng Hiệp, “Chi tiết máy” Nhà xuất giáo dục, 2006 Trịnh Chất, Lê Văn Uyển “Tính tốn, thiết kế hệ thống dẫn động khí” Nhà xuất giáo dục Việt Nam Ngoc-Hien Tran , Hong-Seok Park , Quang-Vinh Nguyen and TienDung Hoang “Development of a Smart Cyber-Physical Manufacturing System in the Industry 4.0 Context” Nguồn Internet Bùi Công Thành, Đỗ Kiến Quốc, Lê Hoàng Tuấn, Nguyễn Thị Hiền Lương, Trần Tấn Quốc “Giáo trình sức bền vật liệu” Nhà xuất Đại học Quốc Gia TP Hồ Chí Minh PGS TS Đào Văn Hiệp, “Kỹ thuật Robot”, Nhà xuất khoa học kỹ thuật, 2006 PGS TS Nguyễn Quang Hoàng, “Bài giảng Robotics”, 2017 87 PHỤ LỤC #include #define rf 200 #define re 100 #define f 21 #define e 10 #define x_f1 12.14 #define z_f1 272 #define x_f2 12.14 #define z_f2 272 #define x_f3 12.14 #define z_f3 272 #define open_angle 50 #define close_angle 80 #define x_g #define z_g 272 #define x_t -90 #define z_t 272 #define DIR1 #define STEP1 #define DIR2 #define STEP2 #define DIR3 #define STEP3 10 88 #define SERVO 11 #define CB 14 #define FET 15 Servo myservo; uint8_t angle_1=0; uint8_t angle_2=0; uint8_t angle_3=0; void solve_equation(int a, int b, int c, int d, int &x,int &z){ float m=(d*d-c*c-b*b)/(2*b); float n=c/b; float delta=4*m*m*n*n-4*(m*m-a*a)*(n*n+1); z=(int)((2*m*n+sqrt(delta))/(2*(n*n+1))); x=(int)(m-n*z); }// giai he pt x^2+z^2=a^2,(x+b)^2+(z+c)^2=d^2 void cal_robot(int x0,int y0,int z0){ int k,h; int x_j1,z_j1,x_j2,z_j2,x_j3,z_j3; solve_equation(rf*rf,f-x0-e,-z0,re*re-y0*y0,k,h); x_j1=k+f; z_j1=h; solve_equation(rf*rf,f-(x0*cos(120*PI/180)-y0*sin(120*PI/180))-e,-z0,re*re-(x0*sin(120*PI/180)+y0*cos(120*PI/180))*(-x0*sin(120*PI/180)+y0*cos(120*PI/180)),k,h); x_j2=k+f; 89 z_j2=h; solve_equation(rf*rf,f-(x0*cos(240*PI/180)-y0*sin(240*PI/180))-e,-z0,re*re-(x0*sin(240*PI/180)+y0*cos(240*PI/180))*(-x0*sin(240*PI/180)+y0*cos(240*PI/180)),k,h); x_j3=k+f; z_j3=h; angle_1=atan2(z_j1,x_j1-x_f1); angle_2=atan2(z_j2,x_j2-x_f2); angle_3=atan2(z_j3,x_j3-x_f3); } void move_robot(int a1, int a2,int a3){ if(a1>angle_1){ digitalWrite(STEP1,HIGH); for(int i=0;i