Tài liệu hạn chế xem trước, để xem đầy đủ mời bạn chọn Tải xuống
1
/ 97 trang
THÔNG TIN TÀI LIỆU
Thông tin cơ bản
Định dạng
Số trang
97
Dung lượng
2,61 MB
Nội dung
NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN NHẬN XÉT CỦA GIÁO VIÊN PHẢN BIỆN MỤC LỤC Trang BẢNG KÝ HIỆU VIẾT TẮT CHƯƠNG 1: TỔNG QUAN VỀ MOBILE ROBOT 1.1- Lịch sử nghiên cứu 1.1.1- Lịch sử phát triển xe AGV 1.1.2- Phân loại xe AGV 1.1.3- Chiến lược phát triển công nghiệp robot quốc gia 11 1.2- Mục tiêu đề tài 15 1.3- Phương pháp thực đề tài 15 CHƯƠNG 2: THIẾT KẾ HỆ THỐNG CƠ KHÍ AGV ROBOT 17 2.1- Lựa chọn phương án thiết kế 17 2.1.1- Lựa chọn phương án thiết kế khí 17 2.1.2- Lựa chọn động 20 2.1.3- Phương án để thiết kế điều khiển 22 CHƯƠNG 3: MÔ HÌNH ĐỘNG HỌC, ĐỘNG LỰC HỌC AGV 26 ROBOT 3.1- Tính tốn động học 26 3.2- Tính tốn động lực học 28 3.3- Tính chọn cấu nâng hạ hàng hóa 33 CHƯƠNG 4- THIẾT KẾ HỆ THỐNG ĐIỆN 36 4.1- Các linh kiện sử dụng 36 4.1.1- Module Wifi ESP32 36 4.1.2- Cảm biến siêu âm 38 4.1.3- Module hạ áp LM2596 39 4.1.4- Module điều khiển động DC BTS 7960 43A 40 4.1.5- Bộ nguồn 43 4.1.6- Mạch dò line 45 4.2- Sơ đồ mạch nguyên lý sơ đồ khối điều khiển 47 CHƯƠNG 5- THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN VÀ THUẬT 52 TOÁN 5.1- Phần mềm lập trình điều khiển 52 5.2- Sơ đồ thuật toán điều khiển 53 5.3- Code điều khiển 53 CHƯƠNG 6- HỒN THIỆN SẢN PHẨM 54 6.1- Hồn thiện lắp ráp khí 54 6.1.1- Lắp ráp khung robot 55 6.1.2- Lắp ráp cụm phận khác 56 6.2- Hồn thiện code nạp chương trình vào vi điều khiển 63 6.3- Chạy thực nghiệm mơ hình robot AGV vận chuyển hàng hóa 63 CHƯƠNG 7: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 64 7.1- Các yêu cầu cho thiết bị 64 7.2- Mức độ hoàn thành 64 7.3- Đánh giá 64 7.4- Kết luận 65 7.5- Hướng phát triển 65 LỜI KẾT 66 TÀI LIỆU THAM KHẢO 67 PHỤ LỤC 68 BẢNG KÝ HIỆU VIẾT TẮT STT Kí hiệu Chú thích q Đơn vị Ma trận vị trí θ Góc robot so với phương ngang Rad ϑ Vận tốc tuyến tính robot m/s Vận tốc bánh phải, trái m/s Vận tốc góc robot Rad/s vR ,vL ω R Bán kính bánh xe mm L Nửa khoảng cách bánh xe mm K tt Động tịnh tiến thân xe J Kb Động quay bánh xe J 10 vt Vận tốc dài xe m/s Vận tốc dài bánh phải, trái m/s 11 v wR , v wL 12 J wR , J wR 13 mb Khối lượng bánh xe Kg 14 mt Khối lượng thân xe Kg 15 M dc Momen động N.m 16 M mst Momen hao tổn trục N.m 17 u 19 Kg/m2 Hệ số tổn thất trục động M msl 18 Momen quán tính bánh k Momen ma sát lăn N.m Hệ số ma sát bánh xe mặt đường 20 g Gia tốc trọng trường m/ s2 21 Vn tc gúc ca bỏnh Rad/s 22 ă Gia tốc góc bánh rad/ s2 Gia tốc dài m/ s2 Vận tốc điểm đầu A, điểm cuối B m/s 23 a Va,Vb 24 25 t Thời gian xe di chuyển s CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT Pdc 26 27 ξ Công suất động Ma trận vị trí robot CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT W CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHƯƠNG 1: TỔNG QUAN VỀ MOBILE ROBOT 1.1- Lịch sử nghiên cứu 1.1.1- Lịch sử phát triển xe AGV Robot đời từ kỷ trước công nguyên Chu Mục Vương (Trung Quốc) người Yan Shi phác thảo ý tưởng máy tự động khí đầu tiên. Đến năm 1927, robot điện tử lần xuất phim ảnh Năm 1948, nguyên lý tảng robot tự động hóa đời, tạo tiền đề cho robot học sau Cho tới năm 1961, Mobile Robot điện tử giới đời, đặt tên Unimate.Trong loại Mobile Robot áp dụng lĩnh vực cơng nghiệp, Robot tự hành mặt đất AGV( Autonomous Guided Vehicles) giới quan tâm đến Hệ thống xe tự hành (AGV) tồn từ năm 1953 Barrett Electronics Of Northbrook, bang Illinois – USA, Savant Automation of Walker, bang Michigan – USA Một nhà phát minh sáng chế phương pháp tự động hoá người xe tải kéo mà sử dụng nhà máy nhiều năm nhờ vào giấc mơ Robot tự hành AGV chế tạo để vận chuyển phôi gia công vào năm 70, vấn đề định hướng xe tự hành vấn đề quan trọng Ban đầu AGV xe kéo nhỏ chạy theo đường dẫn Công nghệ năm 70 điều khiển hệ thống để nâng cao khả tính linh hoạt xe AGV, xe không dùng để kéo, đẩy hàng kho, mà cịn có chức trung gian , kết nối trình sản xuất, lắp ráp, phân loại hàng hóa Trải qua nhiều năm, cơng nghệ phát triển mạnh hơn, q trình tự hành, AGV lập trình để giao tiếp với robot khác nhằm đảm bảo sản phẩm chuyển qua trạm, kho nơi mà sau chúng giữ lại chuyển đến vị trí khác CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT 1.1.2- Phân loại xe AGV Xe AGV sử dụng với mục đích chung để chuyển hàng nhà máy, kho chứa sản phẩm Hình 1.1- Sơ đồ vận hành xe AGV nhà máy Ngày nay, xe AGV có nhiều dịng sản phẩm khác thị trường Các sản phẩm AGV bao gồm: Xe kéo (Towing Vehicle) Xe kéo xuất dòng xe AGV thịnh hành thị trường Loại kéo nhiều toa hàng khác trở từ 8000 đến 60000 pounds Ưu điểm hệ thống xe kéo: Khả chuyên chở lớn; Có thể dự đốn lên kế hoạch tính hiệu việc chuyên chở việc đảm bảo an tồn; Tính an tồn tốt CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT Hình 1.2- Xe AGV kéo hàng Xe chở ( Unit Load Vehicle) Xe chở trang bị tâng khay chứa nâng, hạ chuyênr động băng tải, đai xích Ưu điểm xe chở: Tải trọng phân phối di chuyển theo yêu cầu; Thời gian đáp ứng nhanh gọn; Giảm hư hại tài sản; Đường linh hoạt; Giảm thiếu tắc nghẽn giao thơng chun chở Hình 1.3- Xe AGV chở hàng Xe đẩy ( Cart Vehicle) 10 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT delay(30); if (digitalRead(sw3)==0) { while (digitalRead(sw3)==0); mode=1; vx=0; val=1; tick(1); lcd.clear(); lcd.print(" AUTO MODE "); delay(1000); lcd.clear(); lcd.print(" LINE finding "); while (line()==0) tien(); dung(); tick(2); } } } while (mode==1) { lcd.clear(); lcd.setCursor(0,0);lcd.print("Select: Done: "); lcd.setCursor(15,0);lcd.print(cnt); lcd.setCursor(0,1);lcd.print("From: -> To: "); while(mode==1) { lcd.setCursor(5,1);lcd.print(loca1); lcd.setCursor(14,1);lcd.print(loca2); 83 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT if (SerialBT.available()) { char c=SerialBT.read(); Serial.write(c); if(c=='L') {vx=1;val=1;} else if(c=='H') {loca1++;if(loca1>4) loca1=1;Serial.println(loca1);} else if(c=='I') {loca2++;if(loca2>4) loca2=1;Serial.println(loca2);} else if(c=='K') {vx=0;val=0;} } bool dem=read_sw1(); if(dem==1){ if(millis()-time14) loca1=1; tick(1); time1=millis(); } if (digitalRead(sw2)==0) 84 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT { delay(30); if (digitalRead(sw2)==0) { while (digitalRead(sw2)==0); tick(1); loca2++; if (loca2>4) loca2=1; } } if (digitalRead(sw3)==0) // start { delay(30); if (digitalRead(sw3)==0) { while (digitalRead(sw3)==0); tick(1); if (loca1==0 || loca2==0 || loca1==loca2) tick(2); else { mode=2; vx=1; val=0; // nút start tick(1); lcd.clear(); goto lineFollow; } 85 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT } } if (vx==0 && val==0) //back { mode=0; tick(1); lcd.clear(); } if (vx==1 && val==1) { //run if (loca1==0||loca2==0||loca1==loca2) tick(2); else { mode=2; vx=1; val=0; tick(1); lcd.clear(); goto lineFollow; } } } } lineFollow: while (mode==2) // line Follow { lcd.setCursor(0,0); lcd.print(" RUNNING : "); 86 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT lcd.setCursor(14,0); lcd.print(cnt); lcd.setCursor(0,1); lcd.print("From: -> To: "); lcd.setCursor(5,1); lcd.print(loca1); lcd.setCursor(14,1); lcd.print(loca2); if (loca1==1) { timTRAI(); timNGA4(1); // trai timDUNG(); tick(1); banlen(); delay(2000);bandung(); quaydau(); if (loca2==2) { timNGA4(0); timDUNG(); } else if (loca2==3) { timNGA4(1); timNGA4(1); timDUNG(); } else 87 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT { timNGA4(1); timNGA4(2); timDUNG(); } tick(1); delay(1000); quaydau(); lui(); delay(1000); dung(); banxuong();delay(2000);bandung(); tick(2); lcd.clear(); lcd.print(" GO HOME! "); if (loca2==2) { timNGA4(1); timPHAI(); timDUNG(); } else if (loca2==3) { timNGA4(2); timNGA4(0); timPHAI(); timDUNG(); } 88 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT else { timNGA4(1); timNGA4(0); timPHAI(); timDUNG(); } tick(3); lcd.clear(); lcd.print(" DONE! "); quaydau(); lui(); delay(2000); dung(); tick(3); mode=1; cnt++; break; } else if (loca1==2) { timTRAI(); timNGA4(2); // phai timDUNG(); tick(1); lcd.setCursor(5,1); lcd.print(loca1); banlen(); delay(2000);bandung(); quaydau(); 89 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT if (loca2==1) { timNGA4(0); timDUNG(); } else if (loca2==3) { timNGA4(2); timNGA4(1); timDUNG(); } else { timNGA4(2); timNGA4(2); timDUNG(); } tick(1); lcd.setCursor(14,1); lcd.print(loca2); delay(1000); quaydau(); lui(); delay(1000); dung(); banxuong();delay(2000);bandung(); tick(2); lcd.clear(); lcd.print(" GO HOME! "); if (loca2==1) 90 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT { timNGA4(2); timPHAI(); timDUNG(); } else if (loca2==3) { timNGA4(2); timNGA4(0); timPHAI(); timDUNG(); } else { timNGA4(1); timNGA4(0); timPHAI(); timDUNG(); } tick(3); lcd.clear(); lcd.print(" DONE! "); quaydau(); lui(); delay(2000); dung(); tick(3); mode=1; cnt++; 91 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT Serial1.readString(); break; } else if (loca1==3) { timTRAI(); timNGA4(0); timNGA4(1); // trai timDUNG(); tick(1); lcd.setCursor(5,1); lcd.print(loca1); banlen(); delay(2000);bandung(); quaydau(); if (loca2==1) { timNGA4(2); timNGA4(2); timDUNG(); } else if (loca2==2) { timNGA4(2); timNGA4(1); timDUNG(); } 92 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT else // { timNGA4(0); timDUNG(); } tick(1); lcd.setCursor(14,1); lcd.print(loca2); delay(1000); quaydau(); lui(); delay(1000); dung(); banxuong();delay(2000);bandung(); tick(2); lcd.clear(); lcd.print(" GO HOME! "); if (loca2==1) { timNGA4(2); timPHAI(); timDUNG(); } else if (loca2==2) { timNGA4(1); timPHAI(); timDUNG(); } else // 93 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT { timNGA4(1); timNGA4(0); timPHAI(); timDUNG(); } tick(3); lcd.clear(); lcd.print(" DONE! "); quaydau(); lui(); delay(2000); dung(); tick(3); mode=1; cnt++; break; } else if (loca1==4) { timTRAI(); timNGA4(0); timNGA4(2); // phai timDUNG(); tick(1); lcd.setCursor(5,1); lcd.print(loca1); banlen(); delay(2000);bandung(); quaydau(); 94 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT if (loca2==1) { timNGA4(1); timNGA4(2); timDUNG(); } else if (loca2==2) { timNGA4(1); timNGA4(1); timDUNG(); } else // { timNGA4(0); timDUNG(); } tick(1); lcd.setCursor(14,1); lcd.print(loca2); delay(1000); quaydau(); lui(); delay(1000); dung(); banxuong();delay(2000);bandung(); tick(2); lcd.clear(); lcd.print(" GO HOME! "); if (loca2==1) 95 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT { timNGA4(2); timPHAI(); timDUNG(); } else if (loca2==2) { timNGA4(1); timPHAI(); timDUNG(); } else // { timNGA4(2); timNGA4(0); timPHAI(); timDUNG(); } tick(3); lcd.clear(); lcd.print(" DONE! "); quaydau(); lui(); delay(2000); dung(); tick(3); mode=1; cnt++; break; 96 CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT CHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOTCHUONG.3.mo.HINH.DONG.hoc DONG.luc.hoc.AGV.ROBOT