thiết kế và chế tạo autorobot tham dự abu robocon 2014

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC CẦN THƠ KHOA CÔNG NGHỆ LUẬN VĂN TỐT NGHIỆP ĐẠI HỌC THIẾT KẾ VÀ CHẾ TẠO AUTOROBOT THAM DỰ ABU ROBOCON 2014 CÁN BỘ HƯỚNG DẪN SINH VIÊN THỰC HIỆN Nguyễn Thanh Nhã Đồng Thanh Toàn (1101425) Huỳnh Phước Nhãn (1101405) Trần Trọng Công Minh (1101398) Ngành: Cơ Điện Tử - Khóa 36 Cần Thơ, 05/2014 Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 LỜI CAM ĐOAN Cuộc thi ABU Robocon sân chơi bổ ích cho sinh viên ngành kỹ thuật Thể động, sáng tạo sinh viên để giải tốn thực tế Thơng qua sân chơi Robocon, kinh nghiệm thực tế cá nhân cải thiện phần Vì nhóm chọn đề tài để làm luận văn tốt nghiệp cho mình, vừa thử sức vừa làm tư liệu truyền đạt kinh nghiệm cho hệ u thích Robocon sau Trong q trình thực đề tài, cịn nhiều thiếu sót kiến thức hạn chế kinh nghiệm chưa đủ, nội dung trình bày báo cáo hiểu biết thành nhóm đạt hướng dẫn thầy Nguyễn Thanh Nhã số ý kiến góp ý thầy mơn Nhóm xin cam đoan rằng: nội dung trình bày báo cáo luận văn tốt nghiệp chép từ cơng trình có trước Nếu khơng thật, nhóm xin chịu trách nhiệm trước nhà trường Cần Thơ, ngày 09 tháng 05 năm 2014 Nhóm sinh viên thực hiện: Huỳnh Phước Nhãn Đồng Thanh Toàn Trần Trọng Cơng Minh SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Công Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 LỜI CẢM ƠN Trong sống, khơng có thành cơng mà lại khơng gắn liền với hỗ trợ, giúp đỡ dù hay nhiều, dù trực tiếp hay gián tiếp Để hoàn thành Luận văn tốt nghiệp vậy, nổ lực thân, thành viên nhóm cịn nhận nhiều giúp đỡ từ gia đình, quý thầy bạn bè Vì thế, lời đầu tiên, nhóm xin gửi lời cảm ơn chân thành sâu sắc đến đấng sinh thành cha, mẹ thành viên nhóm Cám ơn cha mẹ có cơng sinh thành, ni dưỡng, chăm lo đời sống cho chúng mặt, hỗ trợ vật chất lẫn tinh thần để chúng có hội học tập ngày hôm Kế đến, nhóm xin chân thành cảm ơn Ban chủ nhiệm Khoa Công Nghệ, Trường Đại học Cần Thơ tạo điều kiện cho nhóm học tập, nghiên cứu, nâng cao kiến thức năm qua Nhóm xin gửi lời cảm ơn đến thầy Nguyễn Thanh Nhã bên cạnh theo dõi, hướng dẫn, hỗ trợ nhiều mặt động viên tinh thần nhóm suốt trình thực đề tài luận văn Bên cạnh nhóm xin gửi lời cảm ơn đến thầy TS Võ Minh Trí trưởng mơn Tự động hóa, thầy TS Ngơ Quang Hiếu phó trưởng mơn Kỹ thuật khí, thầy Nguyễn Văn Khanh, thầy Phạm Trần Lam Hải cho nhóm lời tư vấn hữu ích suốt thời gian thực đề tài Luận văn tốt nghiệp Xin cảm ơn thành viên đội Robocon CT-Hunters, cảm ơn người bạn giúp đỡ cho nhóm suốt thời gian qua, dù hay nhiều góp phần làm nên thành cơng nhóm thực đề tài tốt nghiệp Cuối cùng, xin gửi lời chúc sức khỏe đến quý thầy cô bạn bè, cách chân thành thân ái! SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 NHẬN XÉT CỦA CÁN BỘ HƯỚNG DẪN ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Cần thơ, ngày ……tháng ……năm 2014 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 NHẬN XÉT CỦA CÁN BỘ PHẢN BIỆN ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Cần thơ, ngày ……tháng ……năm 2014 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 NHẬN XÉT CỦA CÁN BỘ PHẢN BIỆN ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… ……………………………………………………………………………………… Cần thơ, ngày ……tháng ……năm 2014 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 MỤC LỤC LỜI CAM ĐOAN LỜI CẢM ƠN NHẬN XÉT CỦA CÁN BỘ HƯỚNG DẪN NHẬN XÉT CỦA CÁN BỘ PHẢN BIỆN NHẬN XÉT CỦA CÁN BỘ PHẢN BIỆN MỤC LỤC DANH MỤC HÌNH ẢNH DANH MỤC BẢNG 10 TÓM TẮT 11 ABSTRACT 12 CHƯƠNG 1: TỔNG QUAN 13 1.1 LỊCH SỬ CUỘC THI ABU ROBOCON 13 1.2 CUỘC THI ABU ROBOCON 2014 15 CHƯƠNG 2: HỆ THỐNG CƠ KHÍ 20 2.1 PHẦN MỀM SỬ DỤNG THIẾT KẾ 20 2.2 THIẾT KẾ AUTOROBOT 21 2.2.1 Yêu cầu nhiệm vụ 21 2.2.2 Thiết kế chi tiết phận Autorobot 22 2.3 MỘT SỐ BỘ PHẬN PHỤ KHÁC 42 2.3.1 Bộ phận dùng để kẹp Autorobot 42 2.3.2 Bộ phận ngắt ống dẫn khí 42 2.4 MỘT SỐ THIẾT BỊ PHỤ TRỢ 43 2.4.1 Bình tích khí 43 2.4.2 Ống dẫn khí nén, ống nối 44 CHƯƠNG 3: HỆ THỐNG ĐIỀU KHIỂN 46 3.1 MẠCH VI ĐIỀU KHIỂN ARDUINO 46 3.1.1 Khối nguồn board 48 3.1.2 Khối vi điều khiển ATmega328 48 3.1.3 Khối giao tiếp IC FT232RL 49 3.2 MẠCH ĐIỀU KHIỂN ĐỘNG CƠ 50 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 3.3 MẠCH ĐIỀU KHIỂN KHÍ NÉN 51 3.3.1 Van đảo chiều 5/2 điều khiển điện 52 3.4 CHUẨN TRUYỀN THÔNG UART VÀ GIAO TIẾP GIỮA HAI ROBOT 53 3.4.1 Chuẩn truyền thông UART 53 3.4.2 Giao tiếp Manualrobot Autorobot 55 3.5 MỘT SỐ LINH KIỆN KHÁC 56 3.5.1 Cơng tắc hành trình (CTHT) 56 3.5.2 Công tắc chọn sân 58 3.5.3 Chổi quét 58 3.5.4 Encoder 59 3.6.1 Pin Lipo 59 CHƯƠNG 4: LẬP TRÌNH CHO AUTOROBOT 61 4.1 CẤU TRÚC CƠ BẢN CỦA MỘT CHƯƠNG TRÌNH ĐƯỢC VIẾT BẰNG ARDUINO IDE 62 4.2 QUY ƯỚC VÀ ĐỊNH NGHĨA 63 4.2.1 Quy ước 63 4.2.2 Định nghĩa 65 4.3 CÁC CHƯƠNG TRÌNH ĐƯỢC DÙNG TRONG PHẦN MỀM ĐIỀU KHIỂN AUTOROBOT 69 4.3.1 CHƯƠNG TRÌNH CON KHỞI TẠO 69 4.3.2 CHƯƠNG TRÌNH CHÍNH 70 4.3.3 CHƯƠNG TRÌNH CON XỬ LÝ TÍN HIỆU XUNG ENCODER 72 4.3.4 CHƯƠNG TRÌNH CON ĐIỀU KHIỂN MOTOR GUỒNG TAY 73 4.3.5 CÁC CHƯƠNG TRÌNH CON THỰC THI TRỰC TIẾP 75 CHƯƠNG 5: KẾT LUẬN 80 5.1 KẾT QUẢ ĐẠT ĐƯỢC 80 5.1.1 Kết thử nghiệm 80 5.1.2 Kết thi đấu 80 5.2 KIẾN NGHỊ 80 PHỤ LỤC 82 TÀI LIỆU THAM KHẢO SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 DANH MỤC HÌNH ẢNH Hình 1.1 Khu vực bập bênh 16 Hình 1.2 Khu vực xích đu 17 Hình 1.3 Khu vực cột 17 Hình 1.4 Khu vực leo thang vận động 18 Hình 2.1 Giao diện Solidwork 2012 20 Hình 2.2 Khối lập phương giới hạn 21 Hình 2.7 Thơng số khung 24 Hình 2.8 Cấu tạo tay kẹp 25 Hình 2.9 Tay kẹp thực tế 25 Hình 2.10 Xi lanh tác dụng lực 26 Hinh 2.11 Xi lanh tay kẹp 26 Hình 2.12 Vị trí tay kẹp đưa vào bập bênh 28 Hình 2.13 Vị trí tay kẹp đưa vào xích đu 28 Hình 2.14 Một số thơng số tay phụ chống nghiêng 29 Hình 2.15 Tay phụ trợ lực 29 Hình 2.16 Nhựa PP 30 Hình 2.17 Kích thước cầu thang theo luật thi 31 Hình 2.18 Ngun lí hoạt động cấu hình bình hành 32 Hình 2.19 Khoảng trượt xống bậc thang cuối leo thang 32 Hình 2.20 Chấu tĩnh bám bậc thang 33 Hình 2.21 Thanh truyền cấu hình bình hành 33 Hình 2.22 Hai truyền 34 Hình 2.23 Cơ cấu chấu động 34 Hình 2.24 Chấu động thực tế 35 Hình 2.25 Kích thước chấu động 35 Hình 2.26 Chấu động bám vào bậc thang 36 Hình 2.27 Cấu tạo truyền động xích cấp 37 Hình 2.28 Sơ đồ truyền động xích hai cấp robot 38 Hình 2.29 Thân Robot lên cao lúc leo thang 39 Hình 2.30 Thân robot nâng lên leo thang 39 Hình 2.31 Động Planet 40 Hình 2.32 Kích thước, thơng số Động DC Planet 30W 41 Hình 2.33 Động lắp giá đỡ 41 Hình 2.34 Nơi kẹp Autorobot 42 Hình 2.35 Cơ cấu ngắt ống dẫn khí 43 Hình 2.36 Bình tích khí nén 43 Hình 2.37 Đầu, co nối ống dẫn khí 44 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 Hình 2.38 Ống dẫn khí nén 44 Hình 3.1 Sơ đồ mạch điện điều khiển Autorobot 46 Hình 3.2 Board arduino nano V3.0 47 Hình 3.3 Arduino Nano Pin Layout 48 Hình 3.4 Sơ đồ ngun lí khối nguồn Arduino nano 48 Hình 3.5 Sơ đồ nguyên lí khối vi điều khiển 49 Hình 3.6 IC giao tiếp 50 Hình 3.7 Mạch PID (mặt sau – mặt trước) 50 Hình 3.8 Sơ đồ mơ điều khiển động 51 Hình 3.9 Sơ đồ mơ tả quy trình điều khiển khí nén 52 Hình 3.10 van khí nén 5/2 van đơn 52 Hình 3.11 Mạch khí nén (2 mặt) 53 Hình 3.12 Tín hiệu tương đương UART RS232 53 Hình 3.13 Truyền bit theo phương pháp song song (a) nối tiếp (b) 54 Hình 3.14 Sơ đồ giao tiếp manual robot auto robot 55 Hình 3.15 LED Rx, Tx board arduino 56 Hình 3.16 Cơng tắc hành trình 56 Hình 3.17 Cơng tắc hành trình tay phụ 57 Hình 3.18 Vị trí cơng tắc chiến thuật 58 Hình 3.19 Chổi quét ( Sau – Trước) 58 Hình 3.20 Encoder từ cấu tạo & thực tế 59 Hình 3.21 Pin Lipo Turnigy 60 Hình 4.1 Trình biên dịch Arduino IDE (giao diện phiên 1.0.5) 61 Hình 4.2 Quy ước cấu tạo Autorobot 63 Hình 4.3 Quy ước tay kẹp trái/phải Autorobot 64 Hình 4.4 Quy ước móc sau 64 Hình 4.5 Quy ước móc trước 64 Hình 4.6 Phân nhóm loại chương trình sử dụng phần mềm điều khiển Autorobot 69 Hình 4.7 Lưu đồ giải thuật chương trình xử lý 71 Hình 4.8 Lưu đồ giải thuật chương trình xử lý tín hiệu xung encoder 72 Hình 4.10 Lưu đồ giải thuật chương trình điều khiển motor guồng tay 74 Hình 4.12 Lưu đồ giải thuật chương trình Chơi xích đu 77 Hình 4.13 Lưu đồ giải thuật chương trình Đi cột 78 Hình 4.14 Lưu đồ giải thuật chương trình leo thang vận động 79 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Trang Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 MỘT SỐ BẢN VẼ KỸ THUẬT AUTOROBOT SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 PHỤ LỤC CÁC MẠCH NGUYÊN LÝ ĐƯỢC SỬ DỤNG CHO AUTOROBOT Hình 3.22 Sơ đồ ngun lý mạch khí nén SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 MÃ NGUỒN PHẦN MỀM ĐIỀU KHIỂN AUTOROBOT #define Unidentify -1 #define Vstop #define Brake #define AntiNoise #define STP 36 0b01010101 #define STR 0b01100100 #define SEE 0b00110101 #define SWG 0b01001101 #define PWK 0b01010011 #define JGM 0b01110101 #define BRK 0b00000010 #define RTF 0b00101010 #define RTB 0b00101001 #define Vadjust 10 #define DForward false #define DBackward true #define Vdisc #define WaitToWalk 3000 #define WaitToClamp 175 #define WaitToUnClamp 400 #define ARed 70 #define Ddisc digitalRead(BlueRed) #define Vjungle 255 SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 #define Vjungle2 25 #define Vjungle3 20 #define Sjungle 800 #define WaitToClimb 1000 #define WaitToCont 200 #define PullTime #define Djungle 400 false ////////////////////Pins definition #define EncoderA #define EncoderB #define M_PWM #define M_DIR #define Rgripper #define Lgripper #define Ltouched #define Rtouched 10 #define Pole4th 11 #define Bholder A0 #define BlueRed A1 #define Fholder #define Shabaash A2 A3 //======================================================= =============== int s=0; byte cmd=0; //======================================================= =============== SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 void PulseIn(); void Rotate(boolean d, byte v, int a); void Seesaw(); void Swing(); void PoleWalk(); void JungleGym(); //======================================================= =============== void setup() { pinMode(Ltouched, INPUT_PULLUP); pinMode(Rtouched, INPUT_PULLUP); pinMode(BlueRed , INPUT_PULLUP); pinMode(Pole4th , INPUT_PULLUP); pinMode(EncoderA, INPUT ); pinMode(EncoderB, INPUT ); pinMode(M_PWM , OUTPUT); pinMode(M_DIR , OUTPUT); pinMode(Rgripper, OUTPUT); pinMode(Lgripper, OUTPUT); pinMode(Bholder , OUTPUT); pinMode(Fholder , OUTPUT); pinMode(Shabaash, OUTPUT); s=0; cmd=0; attachInterrupt(1, PulseIn,FALLING); digitalWrite(Rgripper, HIGH); digitalWrite(Lgripper, HIGH); digitalWrite(Shabaash, HIGH); digitalWrite(Bholder , HIGH); digitalWrite(Fholder , HIGH); Rotate(Ddisc, Vstop, Unidentify); SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 Serial.begin(9600); } //======================================================= =============== void loop() { if (Serial.available()>0){ cmd=Serial.read(); if (cmd==SEE) {cmd=0; Seesaw ();}; if (cmd==SWG) {cmd=0; Swing ();}; if (cmd==PWK) {cmd=0; PoleWalk ();}; if (cmd==JGM) {cmd=0; JungleGym();}; if (cmd==RTF) {cmd=0; Rotate(DForward , Vadjust, Unidentify);}; if (cmd==RTB) {cmd=0; Rotate(DBackward, Vadjust, Unidentify);}; if (cmd==BRK) {cmd=0; Rotate(DForward , Brake , Unidentify);}; if (cmd==STP) {cmd=0; Rotate(DForward , Vstop , Unidentify);};}; } //======================================================= =============== void PulseIn() { if (digitalRead(EncoderB)==LOW) {s ;} else {s++;}; } //======================================================= =============== void Rotate(boolean d, byte v, int a) { if (a>=0) SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 { while (abs(s)!=a) { digitalWrite(M_DIR, d ); analogWrite (M_PWM, 255-v); } Rotate(d, Vstop, Unidentify); } else { digitalWrite(M_DIR, d ); analogWrite (M_PWM, 255-v); } } //======================================================= =============== void Seesaw() { Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Lgripper,LOW ); digitalWrite(Rgripper,HIGH); while(true) { if (Serial.available()>0){cmd=Serial.read();}; if (cmd==STP) {cmd=0; break;}; } digitalWrite(Lgripper,HIGH); digitalWrite(Rgripper,HIGH); } SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 //======================================================= =============== void Swing() { Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Lgripper,LOW ); digitalWrite(Rgripper,LOW ); while(true) { if (Serial.available()>0){cmd=Serial.read();}; if (cmd==STP){cmd=0; break;}} digitalWrite(Lgripper,HIGH); digitalWrite(Rgripper,HIGH); } //======================================================= =============== void PoleWalk() { digitalWrite(Fholder , LOW ); while(true) { if (digitalRead(Ltouched)==LOW ) { delay(AntiNoise); if (digitalRead(Ltouched)==LOW ){break;}; } } digitalWrite(Lgripper, LOW ); digitalWrite(Rgripper, HIGH); while(true) { if (Serial.available()>0){cmd=Serial.read();}; if (cmd==STR) {cmd=0; break;}; } SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 delay(WaitToWalk); Rotate(Ddisc, Vdisc, Unidentify); while(true) { if (digitalRead(Rtouched)==HIGH) { delay(AntiNoise); if (digitalRead(Rtouched)==HIGH){break;} } } while(true) { if (digitalRead(Rtouched)==LOW ) { delay(AntiNoise); if (digitalRead(Rtouched)==LOW ) {break;}; } } digitalWrite(Rgripper, LOW ); delay(WaitToClamp ); Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Lgripper, HIGH); delay(WaitToUnClamp); Rotate(Ddisc, Vdisc, Unidentify); while(true) { if (digitalRead(Ltouched)==HIGH) { delay(AntiNoise); if (digitalRead(Ltouched)==HIGH){break;} } } while(true) { if (digitalRead(Ltouched)==LOW ) { delay(AntiNoise); if (digitalRead(Ltouched)==LOW ) {break;}; } } SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 digitalWrite(Lgripper, LOW ); delay(WaitToClamp ); Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Rgripper, HIGH); delay(WaitToUnClamp); Rotate(Ddisc, Vdisc, Unidentify); while(true) { if (digitalRead(Rtouched)==HIGH) { delay(AntiNoise); if (digitalRead(Rtouched)==HIGH){break;} } } while(true) { if (digitalRead(Rtouched)==LOW ) { delay(AntiNoise); if (digitalRead(Rtouched)==LOW ) {break;}; } } digitalWrite(Rgripper, LOW ); delay(WaitToClamp ); Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Lgripper, HIGH); delay(WaitToUnClamp); if (Ddisc==HIGH){ Rotate(Ddisc, Vdisc, Unidentify); while(true) { if (digitalRead(Pole4th)==LOW ) { SVTH: Huỳnh Phước Nhãn, Đồng Thanh Tồn Trần Trọng Cơng Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 delay(AntiNoise); if (digitalRead(Pole4th)==LOW ) {break; } } }} else{Rotate(Ddisc, Vdisc, ARed);}; Rotate(Ddisc, Vstop, Unidentify); digitalWrite(Fholder, HIGH); delay(AntiNoise); Rotate(Ddisc, Vstop, Unidentify); while(true) { if (Serial.available()>0){cmd=Serial.read();}; if (cmd==STP) {cmd=0; break;}; } digitalWrite(Lgripper, HIGH); digitalWrite(Rgripper, HIGH); } //======================================================= =============== void JungleGym() { while(true) { if (Serial.available()>0){cmd=Serial.read();}; if (cmd==STR) {cmd=0; break;}; } digitalWrite(Lgripper,HIGH); digitalWrite(Rgripper,HIGH); while(true) { if (digitalRead(Ltouched)==LOW ) { delay(AntiNoise); SVTH: Huỳnh Phước Nhãn, Đồng Thanh Toàn Trần Trọng Công Minh Luận văn tốt nghiệp Thiết kế chế tạo Autorobot tham dự thi ABU Robocon 2014 if (digitalRead(Ltouched)==LOW ){break;} if (digitalRead(Rtouched)==LOW ) } { delay(AntiNoise); if (digitalRead(Rtouched)==LOW ){break;} } } digitalWrite(Fholder , LOW ); delay(WaitToClimb); //Start Climbing for (byte i=1; i