BÁO cáo đồ án CÁNH TAY ROBOT 5 bậc tự DO (ROBOT ARM 5 DOF)

TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI TP HỒ CHÍ MINH KHOA ĐIỆN – ĐIỆN TỬ VIỄN THÔNG BÁO CÁO ĐỒ ÁN CÁNH TAY ROBOT BẬC TỰ DO (ROBOT ARM DOF) Ngành: KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HỐ Chun ngành: TỰ ĐỘNG HỐ CƠNG NGHIỆP Giảng viên hướng dẫn: Trương Ngọc Bảo Sinh viên thực hiện: Ngơ Quốc Trưởng Phan Đình Hồi Lê Văn Huy Huỳnh Lê Hoàng Thái Nguyễn Lê Hồng Huy Trần Đăng Khoa TP Hồ Chí Minh, 2021 TIEU LUAN MOI download : skknchat@gmail.com LỜI MỞ ĐẦU Lời đầu tiên, nhóm em xin gửi lời cảm ơn chân thành đến thầy Trương Ngọc Bảo tận tình bảo, giúp đỡ nhóm hồn thành mơn học báo cáo tập lớn môn học Robot Công Nghiệp Với sinh viên kỹ thuật chúng em khoảng thời gian thực hành đồ án sau tiết học lý thuyết điều quý giá Tuy thời gian học ba tháng, nhờ giúp đỡ thầy chúng em học hỏi thêm nhiều điều có nhiều trải nghiệm, kinh nghiệm thực tế, ứng dụng điều học lý thuyết vào thực tiễn biết thêm điều mẻ mà sách chưa thể truyền đạt Với hướng dẫn tận tình thầy Trương Ngọc Bảo, chúng em biết hiểu thứ mà tiếp xúc làm việc sau Theo xu hướng bắt kịp thời đại người không ngừng nâng cao học hỏi, phát triển sống đại, để thỏa mãn nhu cầu đời sống Kiến thức giúp người phát minh đột phá thành tự lớn nhằm tạo sống khơng vất vả Điển hình máy tính, cơng cụ thay đổi giới, góp mặt hầu hết cho cơng việc nhằm mục đích; tính tốn, lưu giữ thơng tin, hỗ trợ liên lạc.v.v…, nhờ cơng việc học tập tiếp cận nhanh Với sứ mệnh học hỏi nghiên cứu nhóm 999 chọn đề tài “Cánh tay robot bậc tự do”, nhằm mục đích tiếp cận với việc thiết kế, mơ vận hành cánh tay robot Thông qua việc thực đồ án sinh viên nhóm có điểm nhìn trực quan robot thiết bị ngoại vi, kết nối khả điều khiển robot Không sinh viên tiếp cận ứng dụng hỗ trợ cho công việc học tập sau Tuy có chuẩn bị báo cáo tập lớn kết thúc môn học xong tránh khỏi sai sót, mong cảm thơng từ thầy Xin chân thành cảm ơn! i TIEU LUAN MOI download : skknchat@gmail.com MỤC LỤC CHƯƠNG LÊN Ý TƯỞNG THỰC HIỆN ĐỒ ÁN 1.1.Đặt vấn đề 1.2.Mục tiêu yêu cầu 1.3.Nội dung nghiên cứu CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1.Giới thiệu Robot 2.1.1 Lịch sử đời Robot 2.1.2 Phân loại Robot 2.1.3 Cánh tay robot 2.1.4 Giải thuật gắn hệ trục tọa độ cho Robot 2.2.Robot bậc tự RRRRR 2.3.Arduino 2.3.1 Phần cứng 2.3.2 Arduino Uno R3 2.4.Động Servo 2.5.Joy stick (Nút nhấn đa hướng) 2.6.Bài toán động học Robot 2.6.1 Một số khái niệm 2.6.2 Biểu diễn Danevit – Hartenberg toán động học Robot: 2.6.3 Giải thuật gắn hệ trục tọa độ cho Robot 2.7.Động học thuận – nghịch cho mơ hình cánh tay 2.7.1 Động học thuận 2.7.2 Động học nghịch CHƯƠNG THIẾT KẾ MÔ HÌNH VÀ MƠ PHỎNG CÁNH TAY ROBOT BẬC TỰ DO TRÊN EASY ROB 3.1.Phần mềm EASY ROB 3.2.Mơ hình Robot thiết kế EASY ROB CHƯƠNG THIẾT KẾ MƠ HÌNH CÁNH TAY ROBOT BẬC TỰ DO TRÊN SOLIDWORKS VÀ MÔ PHỎNG TRÊN MATLAB SIMULINK 4.1.Phần mềm SolidWorks 4.2.Phần mềm Matlab ii TIEU LUAN MOI download : skknchat@gmail.com 4.3 Mơ hình cánh tay robot thiết kế SolidWorks 38 4.4 Mô cánh tay robot với Matlab Simulink 39 4.4.1 Chuyển mơ hình robot từ Solidwork sang Matlab Simulink 39 4.4.2 Tạo giao diện điều khiển robot 39 CHƯƠNG THI CƠNG MƠ HÌNH ROBOT BẬC TỰ DO VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT THEO HAI CHẾ ĐỘ AUTO VÀ MANUAL 42 5.1 Hình ảnh thi cơng mơ hình 42 5.2 Chương trình điều khiển 42 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 54 6.1 Kết luận 54 6.2 Hướng phát triển 54 TÀI LIỆU THAM KHẢO 55 iii TIEU LUAN MOI download : skknchat@gmail.com MỤC LỤC HÌNH Hình 2.1 Kết cấu tay máy Hình 2.2 Tay máy kiểu toạ độ Decartes Hình 2.3 Tay máy kiểu toạ độ trụ Hình 2.4 Tay máy kiểu toạ độ cầu Hình 2.5.SCARA Hình 2.6 Vi điều khiển Arduino UNO R3 12 Hình 2.7 Các cổng I/O Arduino UNO R3 15 Hình 2.8 Servo MG996R 17 Hình 2.9 Servo SG90 17 Hình 2.10 Nút nhấn Joystick 18 Hình 2.11 Kết nối Joystick Arduino UNO R3 18 Hình 2.12 Ma trận tọa độ tuần 23 Hình 2.13 Quy tắc bàn tay phải 23 Hình 2.14 Khâu mơ hình hóa khớp theo phương pháp D-H 24 Hình 2.16 Robot bậc tự biểu diễn không gian 30 Hình 3.1 Giao diện thiết làm việc EASY ROB 33 Hình 3.2 Mơ hình robot thiết kế phần mềm EASY ROB 34 Hình 3.3 Kết sau mơ 36 Hình 4.1 Phần mềm SolidWorks 37 Hình 4.2 Phần mềm Matlab 38 Hình 4.3 Mơ hình cánh tay robot thiết kế solidworks .38 Hình 4.4 Phần mềm MATLAB SOLID WORKS 39 Hình 4.5 Kết sau chuyển đổi 39 Hình 4.6 Giao diện bắt đầu mơ 40 Hình 5.1 Các phần robot động trước lắp ráp 42 Hình 5.2 Robot sau lắp ráp 42 iv TIEU LUAN MOI download : skknchat@gmail.com MỤC LỤC BẢNG Bảng Thông số Arduino Uno R3 12 Bảng Bảng DH Robot Arm DOF 28 v TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo CHƯƠNG LÊN Ý TƯỞNG THỰC HIỆN ĐỒ ÁN 1.1 Đặt vấn đề Thời đại dẫn dầu cơng nghệ, thời đại khoa học cơng nghệ địi hỏi phải phát triển khơng ngừng ln phải tìm tịi học tập để tiến Thiết bị cơng nghệ đổi tiên tiến để góp phần nâng cao chất lượng hiệu an toàn, ổn định thiết bị, máy móc hoạt động Các vi điều khiển ngày ứng dụng lĩnh vực kỹ thuật đời sống xã hội, đặc biệt kỹ thuật tự động hóa điều khiển từ xa “Robot” thuật ngữ khơng cịn xa lạ Robot loại máy đặc biệt, lập trình máy tính có khả thực tự động loạt hoạt động phức tạp điểu khiển máy tính vi mạch điện tử lập trình sẵn Nó cấu tạo phận khí phần mềm – điện tử hoạt động tự động giống người Robot hỗ trợ cho người nhiều nhiều khía cạnh sống sản xuất cơng nghiệp, nơng nghiệp, gia đình, nghiên cứu, giải trí… Trong ngành cơng nghiệp sản xuất dây chuyền ngành cơng nghệ nghiên cứu người bạn hỗ trợ đắc lực cánh tay robot Cánh tay Robot lấy ý tưởng từ cánh tay người, dùng để thực cách hành động thay sức người công việc di chuyển vật, sơn, vặn, hàn, gắp, lắp ráp… Hiện giới, kĩ thuật chế tạo robot phát triển, tích hợp nhiều phụ kiện khác nhau, cánh tay robot đa dạng mẫu mã, kiểu dáng kích thước để phù hợp cho hoàn cảnh, điều kiện sử dụng Tại Việt Nam, có nhiều đơn vị nghiên cứu robot như: Trung tâm tự động hóa, Học viện Kỹ thuật quân sự, Viện nghiên cứu học tin học ứng dụng, Dựa vào kiến thức thân, điều kiện chế tạo ứng dụng robot, tài liệu thu thập được, với giúp đỡ thầy Trương Ngọc Bảo, chúng em xin thực đề tài: “Thiết kế điểu khiển cánh tay Robot bậc tự do” Vì kinh nghiệm chưa nhiều nên trình thực xảy nhiều sai phạm, mong thầy thơng cảm góp ý cho tụi em sửa chữa 1.2 Mục tiêu yêu cầu Mục tiêu đề tài điều khiển cánh tay Robot gắp vật đặt trước tự động gắp gắp vật đặt trước, sau đặt vị trí mục tiêu cho trước Và để cánh tay Robot làm việc ta phải giải số vấn đề cần thiết để xuất trình điều khiển Ví dụ việc giao tiếp mạch Nhóm thực hiện: Nhóm 999 Trang TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo điều khiển Arduino với tay cầm để điều khiển tay với động để điều khiển cách xác Các mục tiêu cần đạt yêu cầu cụ thể đề tài sau: Thiết kế phần cứng cho cánh tay Robot bậc tự Điều khiển cánh tay Robot đến vị trí cho trước cách xác Lập trình điều khiển Robot phần mềm Arduino Robot vận hành hiệu ổn định Yêu cầu phần cứng: Kết cấu vận hành Robot phải chắn, rung lắc Khâu cuối (kẹp) phải đủ lực để gắp vật 1.3 Nội dung nghiên cứu Nghiên cứu lý thuyết robot Thành lập phương trình động học ngược, động học thuận cho robot Sử dụng phần mềm EASY ROB, MATLAB kết hợp SOLID WORDS để mô robot Viết chương trình điều khiển robot cho robot Arduino Nhóm thực hiện: Nhóm 999 Trang TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo CHƯƠNG CƠ SỞ LÝ THUYẾT 2.1 Giới thiệu Robot 2.1.1 Lịch sử đời Robot Năm 1921, Robot biến thể từ gốc Slavo “Rabota” ông Karel Capek (nhà viết kịch viễn tưởng người Séc) sử dụng để đặt tên cho kịch “Rossum’s Universal Robots” Từ dùng để thiết bị lao động người (nhân vật Rossum) tạo Tác giả khoa học viễn tưởng Isaac Asimov cho người sử dụng thuật ngữ “robot” truyện ngắn sáng tác vào năm 1940 Asimov đặt tên cho ngành khoa học nghiên cứu robot Robotics Trong câu chuyện, Asimov đề xuất ba nguyên tắc để hướng dẫn hành vi robot máy thông minh Và gọi tắt “Asimov Ba luật Robotics” tồn đến ngày nay: Robot không xúc phạm người không gây tổn hại cho người Hoạt động robot phải tuân theo quy tắc người đặt Các quy tắc không vi phạm nguyên tắc thứ Một robot cần phải bảo vệ sống mình, khơng vi phạm hai nguyên tắc trước Robotics hiểu ngành khoa học có nhiệm vụ nghiên cứu, thiết kế, chế tạo robot ứng dụng chúng lĩnh vực hoạt động khác xã hội loài người, nghiên cứu khoa học, kỹ thuật, kinh tế, quốc phòng dân sinh Từ hiểu biết sơ chức kết cấu robot, hiểu, Robotics khoa học liên ngành, gồm khí, điện tử, kỹ thuật điều khiển công nghệ thông tin Theo thuật ngữ nay, robot sản phẩm ngành – điện tử (Mechatronics) Khía cạnh nhân văn khía cạnh khoa học – kỹ thuật việc sản sinh robot thống điểm: thực hoài bão người, tạo thiết bị thay hoạt động khơng thích hợp với mình, như: Các cơng việc lặp lặp lại, nhàm chán, nặng nhọc: vận chuyển nguyên vật liệu, lắp ráp, lau cọ nhà… Trong mơi trường khắc nghiệt nguy hiểm: ngồi khoảng không vũ trụ, chiến trường, nước sâu, lịng đất, nơi có phóng xạ, nhiệt độ cao… Những việc địi hỏi độ xác cao, thơng tắc mạch máu ống dẫn thể, lắp ráp cấu tử vi mạch… Ngày nay, robot ứng dụng rộng rãi tiếp tục mở rộng thêm Robot khơng cịn tác lại kết cấu, chức năng, dáng vẻ người, mà có nhiều Nhóm thực hiện: Nhóm 999 Trang TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo robot khác xa với kết cấu phận thể người chúng thực việc vượt xa khả người Drone ví dụ điển hình cho khả Robot 2.1.2 Phân loại Robot Thế giới có nhiều Robot tạo mẫu Robot mang đặc tính mục đích sử dụng riêng đo việc phân loại chúng khơng đơn giản Có nhiều quan điểm để phân loại nêu ba cách phân loại sau: theo kết cấu, theo cách điều khiển phạm vi ứng dụng Robot Phân loại theo kết cấu: Hay cịn gọi phân loại theo hình học, người ta phân loại Robot dựa đặc điểm thành loại Descartes, trụ, cầu, SCARA, kiểu tay người dạng khác Phân loại the phương pháp điều khiển: Có kiểu điều khiển robot: điều khiển hở điều khiển kín Điều khiển hở, dùng truyền động bước (động điện động thủy lực, khí nén…) mà quãng đường góc dịch chuyển tỷ lệ với số xung điều khiển Kiểu đơn giản, đạt độ xác thấp Điều khiển kín (điều khiển kiểu servo), sử dụng tín hiệu phản hồi vị trí để tăng độ xác điều khiển Có kiểu điều khiển servo: điều khiển điểm điểm điều khiển theo đường (contour) Với kiểu điều khiển điểm - điểm, phần công tác dịch chuyển từ điểm đến điểm theo đường thẳng với tốc độ không cao (không làm việc) Nó làm việc điểm dừng Kiểu điều khiển dùng robot hàn điểm, vận chuyển, tán đinh, bắn đinh Điều khiển contour đảm bảo cho phần công tác dịch chuyển theo quỹ đạo bất kỳ, với tốc độ điều khiển Có thể gặp kiểu điều khiển robot hàn hồ quang, phun sơn Phân loại theo ứng dụng: Cách phân loại dựa vào mục đích sử dụng Robot Ví dụ như: robot cơng nghiệp, robot dùng nghiên cứu khoa học, robot dùng kỹ thuật vũ trụ, robot dùng quân sự… Ngoài kiểu phân loại cịn có: Phân loại theo hệ thống lượng, phân loại theo hệ thống truyền động, phân loại theo độ xác… Nhóm thực hiện: Nhóm 999 Trang TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo Kết điều khiển Nhóm thực hiện: Nhóm 999 Trang 42 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo CHƯƠNG THI CƠNG MƠ HÌNH ROBOT BẬC TỰ DO VÀ LẬP TRÌNH ĐIỀU KHIỂN ROBOT THEO HAI CHẾ ĐỘ AUTO VÀ MANUAL 5.1 Hình ảnh thi cơng mơ hình Hình 5.25 Các phần robot động trước lắp ráp Hình 5.26 Robot sau lắp ráp 5.2 Chương trình điều khiển #include #include LiquidCrystal_I2C lcd(0x3F, 16, 2); Nhóm thực hiện: Nhóm 999 Trang 43 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo Servo khop1; Servo khop2; Servo khop3; Servo khop4; Servo khop5; Servo khaukep; int cb = 2; int bientroX = A0 ; int bientroY = A1 ; int bientroZ = A2; int bientrokep = A3; bool mode; float x,y,z; float a1,a2,a3,a4,a5; float d1,d2,d3,d4,d5; float ap1, ap2, ap3, ap4, ap5; float t1, t2, t3, t4, t5; int t1p, t2p, t3p, t4p, t5p; float cos_t3, sin_t3, beta, alpha; float t235 = 0; float d, xd, yd,r4,z4,s; float px,py,pz; int t1d,t2d,t3d,t4d,t5d,gockep; float dophangiai = 0.5; void setup(){ lcd.init(); lcd.backlight(); lcd.setCursor(0, 0); Nhóm thực hiện: Nhóm 999 Trang 44 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo lcd.print("MODE: "); pinMode(12, INPUT_PULLUP); Serial.begin(9600); khop1.attach(5);//0-180 khop2.attach(6);//0-80-150 khop3.attach(9);//0-100 khop4.attach(10);//0-100 khop5.attach(11); khaukep.attach(3); px = 0; py = 6; pz = 26.5; t1p = khop1.read(); t2p = khop2.read(); t3p = khop3.read(); t4p = khop4.read(); t5p = khop5.read(); t1d = 45; t2d = 120; t3d = 90; t5d = 45; runservo(); } void loop(){ //////////////////////////////////////// a1 = 0; a2=12; a3 = 0; a4 = 0; a5 = 4; ap1 = PI/2; ap2 = 0; ap3 = PI/2; ap4=-PI/2;ap5 = 0; d1 = 10.5; d2 = 0; d3 = 0; d4 = 11.8; d5 = 0; mode = digitalRead(12); Nhóm thực hiện: Nhóm 999 Trang 45 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo Serial.print("mode:"); Serial.println(mode); if (mode == LOW){ lcd.setCursor(7, 0); lcd.print("AUTO"); lcd.setCursor(0, 1); lcd.print(" "); automation(); } else if (mode == HIGH){ lcd.setCursor(7, 0); lcd.print("MANU"); lcd.setCursor(0, 1); lcd.print("X: "); lcd.setCursor(5, 1); lcd.print("Y: "); lcd.setCursor(10, 1); lcd.print("Z: "); manual(); } } //////////////////////////////////////// void manual(){ //Hàm điều khiển thủ công int A = analogRead(bientroX); // doc gia tri cua truc X int B = analogRead(bientroY); // doc gia tri cua truc Y int C = analogRead(bientroZ); // doc gia tri cua truc X int D = analogRead(bientrokep); // doc gia tri cua truc X if (A < 50) { Nhóm thực hiện: Nhóm 999 Trang 46 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo px = px - dophangiai; if (px < -27){ px = -27; } } else if (A > 1000){ px = px + dophangiai; if (px > 27){ px = 27; } } ////////// if (B < 50) { py = py - dophangiai; if (py 1000){ py = py + dophangiai; if (py > 27){ py = 27; } } //////// if (C < 50) { pz = pz - dophangiai; if (pz < 0){ pz = 0; } Nhóm thực hiện: Nhóm 999 Trang 47 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo } else if (C > 1000){ pz = pz + dophangiai; if (pz >= 33){ pz = 33; } } if (D < 100) { gockep = gockep - 10; if (gockep 1000){ gockep = gockep + 20; if (gockep > 90){ gockep = 90; } } if ((sqrt(px*px + py*py + pz*pz) < 33) && (sqrt(px*px + py*py) < 27)){ //kiểm tra điều kiện vùng làm việc t1=atan2(py,px); d=sqrt(px*px+py*py); xd=d*cos(t1); yd=d*sin(t1); r4=d-a5*cos(t235); z4=pz-d1+a5*sin(t235); s=sqrt(z4*z4+r4*r4); Nhóm thực hiện: Nhóm 999 Trang 48 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo cos_t3=((s*s-a2*a2-d4*d4)/(2*a2*d4)); sin_t3= sqrt(1-(cos_t3)*(cos_t3)); t3= atan2(sin_t3, cos_t3); beta=atan2(d4*sin(t3),a2+d4*cos(t3)); alpha=atan2(z4,r4); t2=alpha+beta; t5=t235-t2-(-t3); } else { delay(10); } t1d=(180/PI)*t1; t2d=(180/PI)*t2; t3d=(180/PI)*t3; t4d=(180/PI)*t4; t5d=(180/PI)*t5+90; khop1.write(t1d); khop2.write(t2d); khop3.write(t3d); khop4.write(t4d); khop5.write(t5d); khaukep.write(gockep); lcd.setCursor(2, 1); lcd.print(" "); lcd.setCursor(7, 1); lcd.print(" "); lcd.setCursor(12, 1); lcd.print(" "); Nhóm thực hiện: Nhóm 999 Trang 49 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo lcd.setCursor(2, 1); lcd.print(px,0); lcd.setCursor(7, 1);3 lcd.print(py,0); lcd.setCursor(12, 1); lcd.print(pz,0); Serial.print(t1d); Serial.print(";"); Serial.print(t2d); Serial.print(";"); Serial.print(t3d); Serial.print(";"); Serial.print(t4d); Serial.print(";"); Serial.println(t5d); Serial.print(gockep); Serial.print(";"); Serial.print(px); Serial.print(";"); Serial.print(py); Serial.print(";"); Serial.println(pz); delay(100); } void automation(){ //Hàm tự động int gtcb = digitalRead(cb); Serial.println(gtcb); if (gtcb == 0){ t3d = 133; Nhóm thực hiện: Nhóm 999 Trang 50 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo t5d = 160; runservo(); t1d = 0; t2d = 40; runservo(); delay(1000); khaukep.write(0); t2d = 90; runservo(); t5d = 180; runservo(); t1d = 180; t2d = 35; t3d = 129; runservo(); delay(1000); khaukep.write(90); delay(1000); t2d = 90; runservo(); } else if (gtcb == 1){ t1d = 45; t2d = 135; Nhóm thực hiện: Nhóm 999 Trang 51 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo t3d = 90; t5d = 45; runservo(); khaukep.write(90); } } void runservo(){ //Hàm giảm tốc độ quay servo if (t1p > t1d) { for ( int j = t1p; j >= t1d; j ) { khop1.write(j); delay(20); } } if (t1p < t1d) { for ( int j = t1p; j t2d) { for ( int j = t2p; j >= t2d; j ) { khop2.write(j); delay(20); } } if (t2p < t2d) { Nhóm thực hiện: Nhóm 999 Trang 52 TIEU LUAN MOI download : skknchat@gmail.com ROBOT CÔNG NGHIỆP GVHD: ThS Trương Ngọc Bảo for ( int j = t2p; j t3d) { for ( int j = t3p; j >= t3d; j ) { khop3.write(j); delay(20); } } if (t3p < t3d) { for ( int j = t3p; j t5d) { for ( int j = t5p; j >= t5d; j ) { khop5.write(j); delay(15); } } if (t5p < t5d) { for ( int j = t5p; j