Hiện nay ở các vườn ươm cây giống ở nước ta tiêu tốn rất nhiều nhân công lao động nhưng hiệu quả vẫn chưa cao việc ươm trồng các cây giống với kỹ thuật còn khá thô sơ chủ yếu dựa vào kinh nghiệm nên cây giống không được đồng bộ gây khó khăn cho việc gieo trồng chăm sóc và thu hoạch Với mong muốn tạo ra một dây chuyền tự động gieo trồng chăm sóc cây trên các lâm trường với quy mô lớn ứng dụng tiến bộ khoa học kỹ thuật vào trong cuộc sống và sản xuất nhóm chúng tôi đã quyết định chọn đề tài Thiết kế và chế tạo robot dùng trong nông trại ứng dụng công nghệ xử lý ảnh làm đề tài tốt nghiệp
ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP NGÀNH: KỸ THUẬT CƠ ĐIỆN TỬ ĐỀ TÀI: THIẾT KẾ VÀ CHẾ TẠO ROBOT DÙNG TRONG NÔNG TRẠI ỨNG DỤNG CÔNG NGHỆ XỬ LÝ ẢNH Người hướng dẫn: TS ĐẶNG PHƯỚC VINH Người duyệt: TS NGUYỄN DANH NGỌC Sinh viên thực hiện: ĐOÀN ĐẠI THẮNG LÊ CHÍ HIẾU Số thẻ sinh viên : 101140200 101140140 Lớp: 14CDT2 14CDT1 Đà Nẵng, 6/2019 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh TÓM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài: Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh SV thực hiện: Lê Chí Hiếu MSSV: 101140140 Lớp: 14CDT1 Đoàn Đại Thắng MSSV: 101140200 Lớp: 14CDT2 GV hướng dẫn: TS Đặng Phước Vinh GV duyệt: TS Nguyễn Danh Ngọc Nội dung làm bao gồm vấn đề sau: Nhu cầu thực tế đề tài: Hiện vườn ươm giống nước ta tiêu tốn nhiều nhân công lao động hiệu chưa cao, việc ươm trồng giống với kỹ thuật cịn thơ sơ chủ yếu dựa vào kinh nghiệm nên giống không đồng gây khó khăn cho việc gieo trồng, chăm sóc thu hoạch Với mong muốn tạo dây chuyền tự động gieo trồng, chăm sóc lâm trường với quy mô lớn, ứng dụng tiến khoa học kỹ thuật vào sống sản xuất, nhóm chúng tơi định chọn đề tài "Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh" làm đề tài tốt nghiệp Phạm vi nghiên cứu đề tài tốt nghiệp: ✓ Nghiên cứu, thiết kế chế tạo robot tự động gieo trồng, chăm sóc theo dõi điều kiện môi trường xung quanh ✓ Nghiên cứu ứng dụng công nghệ xử lý ảnh vào robot nông nghiệp ✓ Nghiên cứu ứng dụng công nghệ lượng loại robot di động, thân thiện với môi trường ✓ Xây dựng hệ thống cảm biến, lập biểu đồ ghi lại điều kiện khí hậu, thỗ nhưỡng ✓ Nghiên cứu ứng dụng công nghệ thu phát sóng Wifi ✓ Nghiên cứu sử dụng máy tính nhúng Raspberry Pi SVTH: Lê Chí Hiếu Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh i Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh ✓ Thiết kế chế tạo mạch điều khiển động cơ, cảm biến cho robot Nội dung đề tài thực hiện: ✓ Số trang thuyết minh: 67 trang ✓ Số vẽ: vẽ A0 ✓ Mơ hình: Kết đạt được: • Phần lý thuyết ✓ Nghiên cứu, ứng dụng thư viện OpenCV để xử lý hình ảnh ✓ Xử lý ảnh thành cơng việc nhận diện loại trồng hướng di chuyển robot ✓ Thiết kế robot phần mềm Solidwork vẽ vẽ Autocad ✓ Lý thuyết loại cảm biến, NodeMCU, Raspberry Pi ✓ Thiết kế mạch phần mền Proteus, Altium ✓ Lý thuyết nguồn lượng mặt trời ✓ Nghiên cứu, ứng dụng cơng nghệ IoT • Phần tính tốn, thiết kế ✓ Thiết kế chế tạo phần khí ✓ Thiết kế chế tạo mạch điền khiển ✓ Đã chế tạo thành cơng mơ hình hoạt động tương đối ổn định ✓ Tính tốn thiết kế hệ thống cảm biến ✓ Thiết kế giao diện website điện thoại di động Đà Nẵng, ngày tháng năm 2019 Sinh viên thực SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh ii Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh ĐẠI HỌC ĐÀ NẴNG CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT NAM TRƯỜNG ĐẠI HỌC BÁCH KHOA Độc lập - Tự - Hạnh phúc KHOA CƠ KHÍ NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP TT Họ tên sinh viên Số thẻ SV Lớp Ngành Lê Chí Hiếu 101140140 14CDT1 CƠ ĐIỆN TỬ Đoàn Đại Thắng 101140200 14CDT2 CƠ ĐIỆN TỬ Tên đề tài: Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh Đề tài thuộc diện: Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: Tham khảo thực tế Nội dung phần thuyết minh tính tốn: a Phần chung: TT Họ tên sinh viên Nội dung - Tính tốn, thiết kế chế tạo kết cấu khí robot Lê Chí Hiếu - Thiết kế hệ thống mạch điện điều khiển robot - Xây dựng lưu đồ thuật toán điều khiển robot - Xây dựng quy trình xử lý ảnh Đồn Đại Thắng - Nghiên cứu thiết kế quy trình truyền nhận liệu lên Cayene b Phần riêng TT Họ tên sinh viên Nội dung - Nghiên cứu, lập trình xử lý ảnh Lê Chí Hiếu - Nghiên cứu sử dụng Raspberry Pi - Nghiên cứu, thiết kế việc xử lý liệu cảm biến, truyền gửi liệu lên website smartphone - Nghiên cứu, thiết kế mạch sạc pin sử dụng lượng mặt trời Đồn Đại Thắng - Nghiên cứu, lập trình cánh tay Delta - Nghiên cứu việc truyền nhận tín hiệu điều khiển Raspberry Arduino SVTH: Lê Chí Hiếu Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh iii Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh Các vẽ, đồ thị ( ghi rõ loại kích thước vẽ ): a Phần chung: TT Họ tên sinh viên Nội dung Lê Chí Hiếu Bản vẽ tổng thể robot A0 Đoàn Đại Thắng Bản vẽ sơ đồ khối A0 b Phần riêng: TT Họ tên sinh viên Lê Chí Hiếu Đồn Đại Thắng Nội dung Bản vẽ sơ đồ khối mạch điều khiển A0 Bản vẽ khối mạch điện A0 Bản vẽ sơ đồ động A0 Bản vẽ cụm chi tiết A0 Họ tên người hướng dẫn: T.S Đặng Phước Vinh Ngày giao nhiệm vụ đồ án: 1/2/2019 Ngày hoàn thành đồ án: 1/6/2019 Đà Nẵng, ngày tháng năm 2019 Trưởng Bộ môn Kỹ thuật Cơ điện tử SVTH: Lê Chí Hiếu Đồn Đại Thắng Người hướng dẫn GVHD: TS Đặng Phước Vinh iv Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh LỜI NÓI ĐẦU Ngày với ảnh hưởng cách mạng công nghiệp 4.0, ngành Cơ khí nói chung ngành Cơ điện tử nói riêng ngành phát triển mạnh với nhiều thay đổi kỹ thuật công nghệ chất lượng Để đạt điều địi hỏi người cán kỹ thuật ngồi trình độ chun mơn cịn phải có tư sáng tạo, sâu nghiên cứu để tận dụng hết khả năng, nguồn lực Qua thời gian học khoa Cơ khí – Trường Đại học Bách khoa, tận tình hướng dẫn, giúp đỡ thầy, giáo nỗ lực thân, chúng em tích luỹ số kiến thức hành trang để trở thành kỹ sư tương lai Thước đo kiến thức đồ án tốt nghiệp Đó thực thách thức lớn sinh viên chúng em lần đầu tham gia, giải khối lượng công việc lớn Công việc thực đồ án tốt nghiệp gặp vất nhiều khó khăn, nhiều vấn đề nan giải Tuy nhiên giúp đỡ hướng dẫn tận tình thầy Đặng Phước Vinh thầy giáo khác giúp chúng em hồn thành đồ án Nhưng với kiến thức hạn hẹp chưa có kinh nghiệm tính tốn, thi cơng thực tế nên khó tránh sai sót Chúng em kính mong tiếp tục bảo thầy, để em hồn thiện kiến thức Cuối cùng, em xin chân thành cảm ơn thầy, giáo khoa Cơ khí – Trường Đại học Bách khoa – Đại học Đà Nẵng, đặc biệt thầy Đặng Phước Vinh bạn gia đình động viên giúp đỡ chúng em suốt trình thực đồ án Đà Nẵng, ngày tháng năm 2019 Sinh viên Lê Chí Hiếu SVTH: Lê Chí Hiếu Đồn Đại Thắng Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh v Thiết kế chế tạo robot dùng nông trại ứng dụng cơng nghệ xử lý ảnh CAM ĐOAN Kính gửi khoa Cơ khí - Trường đại học Bách khoa - Đại học Đà Nẵng Chúng em xin cam đoan đồ án tuân thủ tốt quy định liêm học thuật: • Khơng bịa đặt, đưa thơng tin sai lệch so với nguồn trích dẫn; • Khơng ngụy tạo số liệu q trình khảo sát, thí nghiệm, thực hành, thực tập hoạt động học thuật khác; • Khơng sử dụng hình thức gian dối việc trình bày, thể hoạt động học thuật kết từ trình học thuật mình; • Khơng đạo văn, sử dụng từ ngữ, cách diễn đạt người khác thể mình, trình bày, chép, dịch đoạn, nêu ý tưởng người khác mà khơng có trích dẫn • Không tự đạo văn, sử dụng lại thông tin nghiên cứu mà khơng có trích dẫn phân mảnh thơng tin kết nghiên cứu để công bố nhiều ấn phẩm Sinh viên thực Lê Chí Hiếu SVTH: Lê Chí Hiếu Đồn Đại Thắng Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh vi Thiết kế chế tạo robot dùng nông trại ứng dụng cơng nghệ xử lý ảnh MỤC LỤC TĨM TẮT ĐỒ ÁN TỐT NGHIỆP i LỜI NÓI ĐẦU .v CAM ĐOAN vi DANH MỤC BẢNG BIỂU x DANH MỤC HÌNH VẼ xi MỞ ĐẦU .1 CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI 1.1 Đặt vấn đề .2 1.2 Giới thiệu tổng quan đề tài .3 1.2.1 Khái quát xử lý ảnh 1.2.2 Tổng quan Internet of Things .4 1.2.3 Giới thiệu nguồn lượng tái tạo .6 1.2.4 Ý tưởng thiết kế robot .7 CHƯƠNG 2: TÍNH TỐN VÀ THIẾT KẾ PHẦN CƠ KHÍ 10 2.1 Cấu tạo robot dùng nông nghiệp: 10 2.2 Phương án truyền động cho bánh xe dẫn động: 11 2.2.1 Phương án 1: Sử dụng động có hộp giảm tốc, truyền đai 11 2.2.2 Phương án 2: Sử dụng động có hộp giảm tốc, truyền xích ngồi 12 2.3 Tính tốn truyền chọn động 13 2.4 Các phương án thiết kế cấu chấp hành cho robot 15 2.4.1 Phương án 1: Sử dụng cánh tay robot làm cấu chấp hành 15 2.4.2 Phương án 2: Sử dụng cánh tay Delta .16 2.5 Tính toán động học cấu cánh tay Delta 16 2.5.1 Động học thuận robot 17 2.5.2 Động học ngược robot .19 CHƯƠNG 3: TÍNH TỐN VÀ THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 22 SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh vii Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh 3.1 Sơ đồ khối mạch điều khiển 22 3.2 Giới thiệu sơ lược linh kiện mạch điều khiển .24 3.2.1 Raspberry Pi model B 24 3.2.2 Camera .25 3.2.3 Tấm pin lượng mặt trời 26 3.2.4 Arduino Uno R3 27 3.2.5 Arduino Nano 29 3.2.6 Node MCU 30 3.2.7 Driver động VNH2SP30 32 3.2.8 Động Servo 32 3.2.9 Mạch hạ áp LM2596 .33 3.2.10 Cảm biến nhiệt độ, độ ẩm DHT11 33 3.2.11 Cảm biến cường độ sáng BH1750 .34 3.2.12 Cảm biến lưu lượng dòng chảy .34 3.2.13 Cảm biến đo dòng ASC712 35 3.3 Thiết kế khối mạch sạc, khối nguồn cho robot 36 3.4 Khối điều khiển động .36 3.5 Khối mạch cảm biến 38 CHƯƠNG 4: THIẾT KẾ CHƯƠNG TRÌNH ĐIỀU KHIỂN .39 4.1 Lưu đồ thuật tốn chương trình điều khiển 39 4.2 Xử lý ảnh .39 4.2.1 Thư viện OpenCV 39 4.2.2 Quy trình xử lý ảnh 40 4.2.3 Chương trình xử lý ảnh 43 4.3 Giao diện người dùng 43 4.3.1 Giới thiệu Cayenne .43 4.3.2 Giao diện website .44 4.3.3 Giao diện smartphone 44 4.4 Chương trình .44 SVTH: Lê Chí Hiếu Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh viii Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh KẾT LUẬN 45 TÀI LIỆU THAM KHẢO 47 PHỤ LỤC 48 PHỤ LỤC 53 SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh ix Thiết kế chế tạo robot dùng nông trại ứng dụng cơng nghệ xử lý ảnh PHỤ LỤC Chương trình điều khiểu cánh tay Detla // -// includes // -#include "Vector3.h" #include "Joint.h" #include "Arm.h" #include "DeltaRobot.h" #include // Change this to your board type #define BOARD_UNO //#define BOARD_MEGA #define Led 13 // Serial communication bitrate const long BAUD = 9600; // Maximum length of serial input message const int MAX_BUF #define TWOPI = 64; (PI*2.0f) #define DEG2RAD (PI/180.0f) #define RAD2DEG (180.0f/PI) #define DELAY (5) // how far should we subdivide arcs into line segments? #define CM_PER_SEGMENT (0.50) #define MIN_FEED_RATE (50) // cm/s //static const float shoulder_to_elbow = 5; // cm //static const float elbow_to_wrist = 18.5f; // cm // //static const float center_to_shoulder = 5.753f; // cm //static const float effector_to_wrist = 1.59258f; // cm float shoulder_to_elbow = 17.5f; // cm float elbow_to_wrist = 35.3f; // cm float effector_to_wrist = 4.7f; //cm SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 53 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh float center_to_shoulder = 9.3f; // cm 8.2841 #ifdef BOARD_UNO static const int pins[] = {9, 10, 11, 3}; #endif #ifdef BOARD_MEGA static const int pins[] = {5, 4, 3, 2}; #endif // -// globals // -long start; // clock time since start long last_frame, this_frame; // for finding dt double dt; char buffer[MAX_BUF]; char buf[100]; int sofar = DeltaRobot robot; float feed_rate = 150; // how fast the tool moves in cm/s // did you put it together backwards? // @TODO: store this in EEPROM int reverse = 0; // @TODO: customize these per-arm? int MAX_ANGLE = 180; int MIN_ANGLE = 20; int val = 0, te = 0; String chuoi; //save point tree float X[10], Y[10],x,y; // -// methods // -/** finds angle of dy/dx as a value from 2PI @return the angle */ float atan3(float dy, float dx) { float a = atan2(dy, dx); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 54 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh if (a < 0) a = (PI * 2.0) + a; return a; } /** setup the geometry of the robot for faster inverse kinematics later */ void setup_robot() { Vector3 temp, n; float aa, bb, cc; int i; for (i = 0; i < NUM_ARMS; ++i) { Arm &a = robot.arms[i]; // shoulder a.shoulder.pos = Vector3(cos(TWOPI * i / NUM_ARMS) * center_to_shoulder, sin(TWOPI * i / NUM_ARMS) * center_to_shoulder, 0); // elbow a.elbow.pos = Vector3(cos(TWOPI * i / NUM_ARMS) * (center_to_shoulder + shoulder_to_elbow), sin(TWOPI * i / NUM_ARMS) * (center_to_shoulder + shoulder_to_elbow), 0); // Find wrist position This is a special case of forward kinematics n = a.shoulder.pos; n.Normalize(); temp = a.shoulder.pos - n * (center_to_shoulder - effector_to_wrist); aa = (a.elbow.pos - temp).Length(); cc = elbow_to_wrist; bb = sqrt((cc * cc) - (aa * aa)); a.wrist.pos = temp + Vector3(0, 0, bb); a.elbow.relative = a.elbow.pos; a.wrist.relative = a.wrist.pos; a.wrist.relative.z = 0; // connect to the servos robot.arms[i].s.attach(pins[i]); // center the arm robot.arms[i].s.write(90); } SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 55 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh robot.default_height = bb; robot.ee.pos.z = bb; /** can the tool reach a given point? @param test the point to test @return 1=out of bounds (fail), 0=in bounds (pass) */ char outOfBounds(float x, float y, float z) { // test if the move is impossible int error = 0, i; Vector3 w, test(x, y, z); float len; for (i = 0; i < NUM_ARMS; ++i) { Arm &arm = robot.arms[i]; // get wrist position w = test + arm.wrist.relative - arm.shoulder.pos; len = w.Length() - elbow_to_wrist if (fabs(len) > shoulder_to_elbow) return 1; } return 0; } /** prints ee position */ void printEEPosition() { Serial.print(robot.ee.pos.x); Serial.print(F("\t")); Serial.print(robot.ee.pos.y); Serial.print(F("\t")); Serial.println(robot.ee.pos.z); } /** sets a servo to a given angle @param angle [0 180] */ void moveArm(int id, float newAngle) { SVTH: Lê Chí Hiếu Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh 56 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh Arm &arm = robot.arms[id]; if (arm.angle == newAngle) return; if (newAngle > MAX_ANGLE) { Serial.println("over max"); newAngle = MAX_ANGLE; } if (newAngle < MIN_ANGLE) { Serial.println("under min"); newAngle = MIN_ANGLE; } arm.s.write(newAngle); arm.angle = newAngle; } /** inverse kinematics for each leg if you know the wrist, it finds the shoulder angle(s) */ void ik() { // find wrist positions int i; for (i = 0; i < NUM_ARMS; ++i) { Arm &arm = robot.arms[i]; // get wrist position arm.wrist.pos = robot.ee.pos + arm.wrist.relative; Vector3 ortho = arm.shoulder.pos; ortho.z = 0; ortho.Normalize(); Vector3 norm(-ortho.y, ortho.x, 0); norm.Normalize(); // get wrist position on plane of bicep Vector3 w = arm.wrist.pos - arm.shoulder.pos; float a = w | norm; // ee' distance Vector3 wop = w - norm * a; arm.wop.pos = wop + arm.shoulder.pos; // use pythagorean theorem to get e'j float b = sqrt(elbow_to_wrist * elbow_to_wrist - a * a); // use intersection of circles to find elbow point (j) //a = (r0r0 - r1r1 + d*d ) / (2 d) SVTH: Lê Chí Hiếu Đoàn Đại Thắng GVHD: TS Đặng Phước Vinh 57 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh float r1 = b; // circle centers on e' float r0 = shoulder_to_elbow; // circle centers on shoulder float d = wop.Length(); // distance from shoulder to the midpoint between the two possible intersections a = ( r0 * r0 - r1 * r1 + d * d ) / ( * d ); // find the midpoint Vector3 n2 = wop; n2.Normalize(); Vector3 temp = arm.shoulder.pos + (n2 * a); // with a and r0 we can find h, the distance from midpoint to intersections float h = sqrt(r0 * r0 - a * a); // get a normal to the line wop in the plane orthogonal to ortho Vector3 r = norm ^ n2; Vector3 p1 = temp + r * h; arm.elbow.pos = p1; // use atan2 to find theta temp = arm.elbow.pos - arm.shoulder.pos; float y = temp.z; temp.z = 0; float x = temp.Length(); if ( ( arm.elbow.relative | temp ) < ) x = -x; float new_angle = atan2(-y, x) * RAD2DEG; // cap the angle if (new_angle > 90) new_angle = 90; if (new_angle < -90) new_angle = -90; // we don't care about elbow angle, but we could find it here if we needed it // update servo to match the new IK data // 2013-05-17 http://www.marginallyclever.com/forum/viewtopic.php?f=12&t=4707&p=5103#p5091 int nx = 90 + ( (reverse == 1) ? new_angle : -new_angle ); /* Serial.print(new_angle); Serial.print("\t"); Serial.print(nx); Serial.print("\n"); //*/ moveArm(i, nx); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 58 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh } } /** moving the tool in a straight line @param destination x coordinate @param destination y coordinate @param destination z coordinate */ void line(float x, float y, float z) { if ( outOfBounds(x, y, z) ) { Serial.println("Out of bounds."); return; } // how long does it take to reach destination at speed feed_rate? Vector3 destination(x, y, z); Vector3 start = robot.ee.pos; // keep a copy of start for later in this method Vector3 dp = destination - start; // far we have to go? float travel_time = dp.Length() / feed_rate; travel_time *= 1000; // convert to ms // save the start time of this move so we can interpolate linearly over time long start_time = millis(); long time_now = start_time; /* Serial.print(F("length=")); Serial.println(dp.Length()); Serial.print(F("time=")); Serial.println(travel_time); Serial.print(F("feed=")); Serial.println(feed_rate); printEEPosition(); */ // we need some variables in the loop Declaring them outside the loop can be more efficient float f; // until the interpolation finishes while (time_now - start_time < travel_time) { // update the clock time_now = millis(); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 59 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh // find the point between destination and start that we've reached // this is linear interpolation f = (float)(time_now - start_time) / travel_time; robot.ee.pos = dp * f + start; //printEEPosition(); // update the inverse kinematics ik(); delay(DELAY); } // one last time to make sure we hit right on the money robot.ee.pos = destination; // update the inverse kinematics ik(); } /** subdivides a line into shorter segments for straighter motion @param destination x coordinate @param destination y coordinate @param destination z coordinate */ static void line_safe(float x, float y, float z) { // split up long lines to make them straighter? float dx = x - robot.ee.pos.x; float dy = y - robot.ee.pos.y; float len = sqrt(dx * dx + dy * dy); if (len 0) angle1 += TWOPI; theta = angle2 - angle1; // get length of arc // float circ=PI*2.0*radius; // float len=theta*circ/(PI*2.0); // simplifies to float len = abs(theta) * radius; int i, segments = floor( len / CM_PER_SEGMENT ) float nx, ny, nz, angle3, scale; for (i = 0; i < segments; ++i) { // interpolate around the arc scale = ((float)i) / ((float)segments); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 61 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh angle3 = ( theta * scale ) + angle1; nx = cx + cos(angle3) * radius; ny = cy + sin(angle3) * radius; nz = ( z - robot.ee.pos.z ) * scale + robot.ee.pos.z; // send it to the planner line(nx, ny, nz); } line(x, y, z); } void nhapnhay() { for (int i = 0; i < 5; i++) { digitalWrite(Led, HIGH); delay(100); digitalWrite(Led, LOW); delay(100); } } void testArm(int id) { float oldAngle = robot.arms[id].angle; moveArm(id, MIN_ANGLE); delay(1000); moveArm(id, MAX_ANGLE); delay(1000); moveArm(id, MIN_ANGLE); delay(1000); moveArm(id, MAX_ANGLE); delay(1000); // moveArm(id, oldAngle); // delay(500); } void testWave() { // sine curve from MIN_ANGLE to MAX_ANGLE over time float speedup = 2; int HALF_RANGE = ( MAX_ANGLE + MIN_ANGLE ) / 2; int MIDDLE_ANGLE = MIN_ANGLE + HALF_RANGE; SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 62 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh while (1) { float t = millis() * 0.001f; /* int i; for(i=0;i= (maxx / 2)) { aa = maps(a, maxx / 2, maxx, 0, -14.9); return aa; } } if (b == 'Y') { if (a < (maxy / 2)) { aa = maps(a, 0, maxy / 2, 12.4, 0); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 63 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh return aa; } if (a >= (maxy / 2)) { aa = maps(a, maxy / 2, maxy, 0, -12.4); return aa; } } } /** process instructions waiting in the serial buffer */ void processCommand() { if (!strncmp(buf, "M114", 4)) { // get position printEEPosition(); } else if ( !strncmp(buf, "RV", 2)) { reverse = (reverse == 1) ? : 1; } else if ( chuoi == "TEST0") testArm(0); else if ( chuoi == "TEST1") testArm(1); else if ( chuoi == "TEST2") testArm(2); else if ( !strncmp(buf, "TESTWAVE", 8)) testWave(); else if ( !strncmp(buf, "G00", 3) || !strncmp(buf, "G01", 3) ) { // line float xx = robot.ee.pos.x; float yy = robot.ee.pos.y; float zz = robot.ee.pos.z; //- robot.default_height; //float ff = feed_rate; float ff = 200; te=0; char *ptr = buf; while (ptr && ptr < buf + sofar) { ptr = strchr(ptr, ' ') + 1; switch (*ptr) { case 'X': xx = atof(ptr + 1); y = px2cm(xx, 'X'); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 64 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh Y[val] = y; Serial.print('Y'); Serial.println(y); break; case 'Y': yy = atof(ptr + 1); x = px2cm(yy, 'Y'); X[val] = x; Serial.print('X'); Serial.println(x); break; case 'Z': zz = atof(ptr + 1); Serial.print('z'); Serial.println(zz); break; case 'F': ff = atof(ptr + 1); Serial.print('f'); Serial.println(ff); break; default: ptr = 0; break; } } for (int j = 0; j < val; j++) { if ((((X[j] - 0.5 < x) && (x < X[j] + 0.5)))&& ((Y[j] - 0.5 < y) && (y < Y[j] + 0.5))) te = te + 1; } //Serial.println(te); if (te == 0) { X[val] = x; Y[val] = y; val++; //Serial.print(val); } if (feed_rate < MIN_FEED_RATE) feed_rate = MIN_FEED_RATE; feed_rate = ff; // line_safe(x, y, 25); if (val == 3) { for (int i = 0; i < val; i++) { Serial.print('X'); Serial.println(Y[i]); Serial.print('Y'); Serial.println(X[i]); line_safe(X[i],Y[i], 30); delay(800); // line_safe(X[i],Y[i], 30); // delay(2000); // line_safe(X[i],Y[i], 20); SVTH: Lê Chí Hiếu Đồn Đại Thắng GVHD: TS Đặng Phước Vinh 65 Thiết kế chế tạo robot dùng nông trại ứng dụng công nghệ xử lý ảnh // delay(1000); // X[i] = 0; Y[i] = 0; } val = 0; // Thực bơm di chuyển xe } } } void setup() { // initialize the read buffer sofar = 0; // start serial communications Serial.begin(BAUD); Serial.println(F("\n\nHELLO WORLD! I AM A DELTA ROBOT.")); Serial.println("Receiving data"); pinMode(Led, OUTPUT); setup_robot(); // @TODO: Is this necessary? //line(0,0,0); //Serial.print(F("> ")); start = millis(); Serial.println(); } void loop() { // put your main code here, to run repeatedly: while ( Serial.available()) { // buffer[sofar++]=Serial.read(); // if(buffer[sofar-1]==';') break; chuoi = Serial.readStringUntil('\n'); Serial.println(chuoi); sofar = 0; //COVER STRING TO CHAR// for (byte len = 1; len