Robot hút bụi tự động

Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức Mục lục PHẦN MỞ ĐẦU Lí chọn đề tài Mục đích nghiên cứu Cách tiếp cận Đối tượng nghiên cứu, phạm vi nghiên cứu 4.2 Phạm vi nghiên cứu Phương pháp nghiên cứu chế tạo 6 Cấu trúc đồ án CHƯƠNG I: TỔNG QUAN VỀ ROBOT HÚT BỤI 1.1 Robot ứng dụng robot tự động thực tiễn 1.2 Cơ sở lí thuyết 1.2.1 Khái quát robot hút bụi tự động 1.2.2 Nguyên lí làm việc robot hút bụi tự động sử dụng cảm biến siêu âm ……………………… 10 1.2.3 Một số linh kiện sử dụng đề tài 11 CHƯƠNG II: PHƯƠNG PHÁP VÀ NỘI DUNG NGHIÊN CỨU 19 2.1 Ý tưởng thiết kế 19 2.2 Sơ đồ khối ………………… 19 2.2.1 Khối thông tin 20 2.2.2.Khối xử lí 24 2.2.3 Khối cảm biến 29 2.2.3 Khối chấp hành 37 2.2.4 Khối nguồn 40 2.3 Sơ đồ mạch tránh vật cản 42 2.3.1 Sơ đồ lắp ráp 43 2.3.2 Lưu đồ thuật toán tránh vật cản 44 2.4 Code robot tránh vật cản 45 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức 2.5 Vật tư linh kiện 45 CHƯƠNG III: THỰC NGHIỆM VÀ ĐÁNH GIÁ KẾT QUẢ 46 3.1 Mục đích kiểm nghiệm 46 3.2 Tiến trình kiểm nghiệm 46 3.3 Kiểm nghiệm trước hoạt động 46 3.4 Thử nghiệm khả di chuyển tránh vật cản 47 3.5 Đánh giá 49 CHƯƠNG IV KẾT QUẢ ĐẠT ĐƯỢC VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI 50 4.1 Kết qủa đạt 50 4.1.2 Kết chưa đạt 53 TÀI LIỆU THAM KHẢO 54 PHỤ LỤC: 55 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức Danh mục hình ảnh sử dụng báo cáo Hình 1 Arduino UNO R3 12 Hình Cảm biến siêu âm SRF-04 12 Hình Module điều khiển động L298 14 Hình Động giảm tốc V1 14 Hình Động RC Servo 9G 15 Hình 6.Pin sạc Panasonic 15 Hình Mạch thu phát Bluetooth HC – 05 17 Hình Module Relay với Opto cách ly kích H/L(5V DC) 18 Hình 2.1 : Sơ đồ khối hệ thống 19 Hình 2.2 : Cấu trúc mạch khối thơng tin 20 Hình 2.3 Giao diện quản lý Project Mit appeventor…………………… .21 Hình 4.Giao diện thiết kế 21 Hình Giao diện lập trình (Blocks) 22 Hình Block điều khiển thiết bị giọng nói sau lập trình 23 Hình 7.Sơ đồ kết nối khối thông tin 23 Hình Code điều khiển thiết bị giọng nói 25 Hình Arduino Uno R3 25 Hình 10 Các chân nguồn Arduino Uno R3 26 Hình 11 Các chân vào/ra Arduino Uno R3 27 Hình 12 Giao diện lập trình cho Arduino Uno R3 29 Hình 13 Hình ảnh thực tế cảm biến siêu âm SRF 04 30 Hình 2.14 Sơ đồ kết nối arduino với cảm biến siêu âm SRF04 31 Hình 2.15 Code cho cảm biến siêu âm 32 Hình 15 Các khoảng cách đo từ cảm biến siêu âm 32 Hình 16 Động Servo 33 Hình 17 Cách nối dây servo với arduino 36 Hình 18 Cách nối dây khối cảm biến 37 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hồng Anh Đức Hình 19 Hình ảnh thực tế khối cảm biến 37 Hình 20.Hình ảnh thực tế Module L298 38 Hình 21 Hình ảnh thực tế động giảm tốc V1 38 Hình 22 Sơ đồ lắp ráp .42 Hình 23 Lưu đồ thuật toán 43 Hình 24 Code tránh vật cản 43 Hình 3.1 Giao diện điều khiển Smart Phone 46 Hình 3.2 Robot vị trí ban đầu 46 Hình 3.3 Robot gặp vật cản 47 Hình 3.4 Robot rẽ trái tiếp tục chuyển động 47 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức PHẦN MỞ ĐẦU Lí chọn đề tài Trong sống robot thay thế, trợ giúp người vận chuyển hàng hóa, robot kiểm tra nguy hiểm,robot phục vụ cho cơng việc gia đình phổ biến Nhất robot tự hoạt động mà khơng cần người điều khiển giúp giảm công việc cho người Cùng với phát triển khoa học, công nghệ việc thiết kế robot làm việc nhà điều nên làm Từ ý tưởng em chọn đề tài “ robot hút bụi tự động” cho đồ án Tính đề tài Robot hút bụi tự động robot di chuyển tự động mà không cần người phải điều khiển , tự động tránh vật cản từ hút bụi tất vị trí nhà Robot tránh vật cơng nghệ dùng mạch logic áp dụng từ sớm thu số thành tựu định đồng thời xuất số nhược điểm làm cho robot khó xác định vật cản di chuyển tránh vật cản cách xác Để khắc phục nhược điểm đồ án xây dựng thiết kế robot tránh vật cản, tín hiệu từ cảm biến siêu âm chuyển cổng điều khiển vi điều khiển, tín hiệu xử lí đưa tín hiệu để điều khiển cho cấu chấp hành động servo, moudule điều khiển động cơ,…Trong đồ án sử dụng moudule vi điều khiển Arduino Uno R3 moudule thơng dụng dễ dàng lập trình Ngồi phần hút bụi sử dụng vật liệu có sẵn quạt thơng gió máy tính, vải…Rất dễ tìm kiếm thị trường dễ chế tạo, lắp ráp Mục đích nghiên cứu Thiết kế hồn chỉnh đưa quy trình làm mơ hình robot hút bụi tự động có khả tránh vật cản nhà vàvà hoạt động ổn định Cách tiếp cận SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức Đồ án tiếp cận kết số công trình chế tạo robot cơng bố : lí thuyết chế tạo robot, sở liệu nguyên lí hoạt động số linh kiện điện tử, số máy đo chuyên dùng, phần mềm lập trình mơ Để từ đề xuất ngun lí đưa quy trình thực đồ án Đối tượng nghiên cứu, phạm vi nghiên cứu 4.1 Đối tượng nghiên cứu  Robot tránh vật cản  Máy hút bụi  Hệ thống cảm biến khoảng cách  Module điều khiển động Arduino Motor Shield Rev3  Cách giao tiếp Arduino UNO R3 với hệ thống cảm biến với moudule điều khiển động Arduino Motor Shield Rev3 4.2 Phạm vi nghiên cứu Nghiên cứu, thiết kế máy hút bụi tự động có khả hút bụi tự động tránh vật cản sử dụng gia đình Phương pháp nghiên cứu chế tạo  Phương pháp chuyên gia: Hỏi ý kiến chuyên gia lĩnh vực nghiên cứu - Phương pháp nghiên cứu lí thuyết: Nghiên cứu tài liệu liên quan đến đề tài  Phương pháp nghiên cứu thực tiễn: - Mô proteus phần mềm arduino - Sử dụng module có sẵn cách tối ưu, hợp lí - Làm thân robot theo phương pháp gia cơng khí - Phương pháp viết code phần mềm có sẵn arduino Cấu trúc đồ án Ngoài phần mở đầu kết luận đồ án gồm chương: Chương Tổng quan robot hút bụi Chương Phương pháp nội dung nghiên cứu Chương Thực nghiệm đánh giá kết Chương Kết luận hướng phát triển đề tài SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức CHƯƠNG I: TỔNG QUAN VỀ ROBOT HÚT BỤI 1.1 Robot ứng dụng robot tự động thực tiễn Hiện với cách mạng khoa học- kĩ thuật 4.0 thời gian tới nhà hay người sử dụng robot để giúp việc cho người từ người nghỉ ngơi, giải trí điều không xa lạ Với xu này, với ứng dụng truyền thống khác robot công nghiệp, y tế, giáo dục đào tạo, giải trí đặc biệt an ninh quốc phịng thị trường robot dịch vụ ăn theo robot vô lớn Robot tổng hợp khoa học công nghệ Để thiết kế chế tạo robot, ta cần có tri thức tốn học, học, vật lý, điện tử, lý thuyết điều khiển, khoa học tính tốn nhiều tri thức khác Để ứng dụng robot, ta cần biết rõ đối tượng ứng dụng Robot sản phẩm tích hợp khoa học công nghệ với độ phức tạp cao Cơng nghệ có tiến đáng kể nửa kỷ qua Robot ứng dụng công nghiệp vào năm 60 để thay người làm công việc nặng nhọc, nguy hiểm môi trường độc hại Do nhu cầu cần sử dụng ngày nhiều trình sản xuất phức tạp nên robot cơng nghiệp cần có khả thích ứng linh họat thơng minh Ngày nay, ứng dụng sơ khai ban đầu robot chế tạo máy ứng dụng khác y tế, chăm sóc sức khỏe, nơng nghiệp, đóng tàu, xây dựng, an ninh quốc phịng gia đình có nhu cầu gia tăng động lực cho robot địa hình robot dịch vụ phát triển Có thể kể đến số loại robot quan tâm nhiều thời gian qua là: tay máy robot (Robot Manipulators), robot di động (Mobile Robots), robot sinh học (Bio Inspired Robots) robot cá nhân (Personal Robots) Tay máy robot bao gồm loại robot công nghiệp (Industrial Robot), robot y tế (Medical Robot) robot trợ giúp người tàn tật (Rehabilitation robot) Robot di động nghiên cứu nhiều xe tự hành mặt đất AGV (Autonomous Guided Vehicles), robot SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức tự hành nước AUV (Autonomous Underwater Vehicles), robot tự hành không UAV (Unmanned Arial Vehicles) robot vũ trụ (Space robots) Với robot sinh học, nghiên cứu thời gian qua tập trung vào loại robot (Walking robots) robot dáng người (Humanoid Robots) Bên cạnh đó, loại robot sinh học nước robot cá, cấu trúc chuyển động theo sinh vật biển nhiều nhóm nghiên cứu phát triển Trong thời kì phát triển khoa học cơng nghệ chiếm vị trí quan trọng việc điều chỉnh phát triển ngành khác Sự phát triển khoa học công nghệ, đặc biệt cách mạng công nghiệp lần thứ lần thứ hai mở đường cho hình thành phát triển nhanh chóng thị trường giới Nếu cách mạng công nghiệp lần thứ chủ yếu dựa vào nước, sắt, than chì cách mạng công nghiệp lần thứ hai chủ yếu dựa vào máy chạy nhiên liệu lượng nguyên tử Về lĩnh vực điện tử tin học: lĩnh vực thu hút nhiều người quan tâm, tốc độ phát triển ngày nhanh mạnh Về lĩnh vực tự động hoá: ngày sử dụng nhiều máy móc, cơng cụ điều khiển số, robot lao động, sản xuất phục vụ đời sống Để đáp ứng yêu cầu ngày cao xã hội với mục tiêu “giáo dục quốc sách hàng đầu” hướng tới cải cách giáo dục toàn diện để phát triển cho hệ tương lai Khoa kỹ thuật – công nghệ thông tin trường đại học Quảng Bình cung cấp cho sinh viên kiến thức kĩ thuật khái niệm, nguyên lí, linh kiện điện tử ứng dụng thực tế…Với linh kiện có sẵn khoa thị trường khối lượng kiến thức học thời gian qua việc thiết kế robot tự động hút bụi điều hoàn toàn làm SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp 1.2 Cơ sở lí thuyết GVHD: Th.S Hoàng Anh Đức 1.2.1 Khái quát robot hút bụi tự động a Khái niệm Robot hút bụi tự động robot tự động di chuyển, hút bụi mà khơng cần người điều khiển - tự động tránh vật cản hút bụi Robot hút bụi tự động bao gồm: vi điều khiển, moudule điều khiển động cơ, động cơ, cảm biến phần hút bụi b Nhiệm vụ Robot tự động hút bụi có tránh vật cản nhà từ di chuyển xung quanh nhà hút bụi c Phân loại robot hút bụi tự động Phân loại robot hút bụi tự động dựa theo nguyên lý làm việc đầu cảm biến có:  Robot tránh vật cản sử dụng cảm biến hồng ngoại: robot tránh vật cản làm việc dựa vào nguyên lí làm việc cảm biến hồng ngoại Robot chủ yếu phát vật cản nhờ sóng hồng ngoại  Robot tránh vật cản sử dụng cảm biến siêu âm: robot tránh vật cản làm việc dựa vào nguyên lí làm việc cảm biến siêu âm Robot chủ yếu phát vật cản nhờ sóng siêu âm  Robot tránh vật cản sử dụng cảm biến laser: robot tránh vật cản làm việc dựa vào nguyên lí làm việc cảm biến laser Robot chủ yếu phát vật cản nhờ laser  Robot tránh vật cản sử dụng nhiều loại cảm biến: hệ thống làm việc dựa nguyên lí nhiều loại cảm biến như: cảm biến hồng ngoại cảm biến siêu âm, cảm biến siêu âm cảm biến laser Robot chủ yếu phát vật cản môi trường khác SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức Phân loại robot theo chuyển động ta chia robot tự hành thành loại: chuyển động chân bánh  Robot chuyển động chân robot di chuyển mô theo di chuyển người Tuy nhiên phức tạp di chuyển số địa hình định  Robot chuyển động bánh làm việc tốt hầu hết địa hình người tạo Điều khiển di chuyển bánh đơn giản nhiều, gần ln đảm bảo tính ổn định Lớp chuyển động bánh chia làm loại:loại chuyển động bánh xe, loại chuyển động vòng xích loại hỗn hợp bánh xe vịng xích Tiềm ứng dụng xe tự hành lớn, kể đến robot vận hành vật liệu, hàng hóa tòa nhà, nhà máy, cửa hàng, sân bay hay thư viện… robot xe lăn phục vụ người khuyết tật,… 1.2.2 Nguyên lí làm việc robot hút bụi tự động sử dụng cảm biến siêu âm a Các trạng thái làm việc robot tránh vật cản sử dụng cảm biến siêu âm  Trạng thái thường trực (khi khơng có vật cản): Robot đưa khoảng cách an toàn đồng thời cảm biến siêu âm bắt đầu đo khoảng cách  Trạng thái di chuyển: Robot thẳng dến gặp vật cản  Trạng thái có vật cản: Robot tiến hành đo khoảng cách hai bên từ xác định hướng di chuyển hợp lí b Nguyên lí làm việc robot tự động sử dụng cảm biến siêu âm Bình thường robot hoạt động chế độ thường trực Ở chế độ robot ln có tín hiệu kiểm tra làm việc đến thiết bị robot cảm biến,các module,…từ cảm biến moudule có tín hiệu hồi đáp trung tâm Trong robot ln có dịng điện I0 chạy qua SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 10 Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức 3.5 Đánh giá Ưu điểm:  Xây dựng thành công phần mềm điều khiển thiết bị giọng nói thơng qua Smart Phone  Robot hoàn thành hoạt động theo yêu cầu đặt  Robot nhỏ gọn dễ dàng di chuyển Nhược điểm:  Nếu chướng ngại vật bé ngồi tầm qt cảm biến cảm biến không hoạt động dẫn đến robot chạy sai  Hút bụi cơng suất cịn yếu  Chưa có phần mô đường lên đường hút bụi ngẫu nhiên 49 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức CHƯƠNG IV KẾT QUẢ ĐẠT ĐƯỢC VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI 4.1 Kết qủa đạt - Đề tài nghiên cứu phương pháp điều khiển thiết kế, chế tạo robot hút bụi dựa nhu cầu thực tiễn.Robot hút bụi đạt yêu cầu đề - Mơ hình hồn thiện bao gồm phần thiết kế khí, mạch điện tử chương trình điều khiển Nghiên cứu ứng dụng vi điều khiển Arduino việc điều khiển robot hút bụi tự động thành công - Sử dụng Smart Phone điều khiển robot thông qua giọng nói tiện lợi - Hồn thành app điều khiển giọng nói Smart Phone Kết : Mơ hình robot hút bụi hoạt động tốt theo yêu cầu thiết kế đặt 4.1.1 Một số hình ảnh đề tài Hình 3.5 Tổng thể mơ hình 50 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hồng Anh Đức Hình 3.6 Động bánh tạo chuyển động cho xe Hình 3.7 Hệ thống cảm biến bánh điều hướng 51 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hồng Anh Đức Hình 3.8 Nguồn mơ phận hút bụi Hình 3.9 Bố trí linh kiện xe 52 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức 4.2.Kết chưa đạt - Robot hạn chế hoạt động địa hình nhấp nhơ - Dung lượng Pin khiêm tốn - Lặp lại diện tích làm việc - Robot chưa thể tự xạc nguồn  Kết luận Sau thời gian nghiên cứu, đề tài hoàn thiện đáp ứng đầy đủ số yêu cầu ban đầu đặt cụ thể:  Nghiên cứu robot tránh vật cản sở thực tiễn sở lí thuyết thiết kế robot tránh vật cản sử dụng cảm biến siêu âm  Chế tạo thành công robot tránh vật cản sử dụng cảm biến siêu âm  Nghiên cứu, chế tạo thành cơng mơ hình máy hút bụi  Robot hút bụi tự động chạy ổn định  Hướng phát triển đề tài  Lắp thêm cảm biến để robot hoạt động tốt khắc phục nhược điểm robot cũ robot tự trở chỗ sạc pin pin yếu  Thêm thuật toán di chuyển để robot di chuyển tối ưu 53 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức TÀI LIỆU THAM KHẢO [1] Nguyễn Minh Hà ( 2008), Kỹ thuật mạch điện tử, NXB Khoa học &Kĩ thuật Hà Nội [2] Nguyễn Thị Mai Lan (2015), Tập giảng Kĩ thuật tương tự,Khoa Sư phạm kĩ thuật, Đại học Sư phạm Hà Nội [3] Nguyễn Đức Phô (chủ biên), Nguyễn Đức Chiến (2009), Giáo trình cảm biến, NXB Khoa học kĩ thuật [4] Lê Thế Quang (Chủ biên) ,Thực hành kĩ thuật điện tử , NXB Đại học sư phạm Hà Nội [5] www.alldatasheet.com, truy cập cuối ngày 24/12/2018 [6] Tài liệu internet 54 SVTH: Nguyễn Hữu Tuấn Sibunhương Settha Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức PHỤ LỤC: Code điều khiển giọng nói: String data; int led1 = 12; int led2 = 13; void setup() { Serial.begin(9600); pinMode(led1, OUTPUT); pinMode(led2, OUTPUT); } void loop() { while (Serial.available()){ //Kiểm tra byte để đọc delay(30); //Delay để ổn định char c = Serial.read(); // tiến hành đọc if (c == '#') {break;} //Thoát khỏi vòng lặp phát từ # data += c; // data = data + c } if (data.length() > 0) { Serial.println(data); if(data == "bật máy" ||data == "Bật máy"||data == "1") {digitalWrite(led1, HIGH);} ////////////////////////////////////////////////////// else if(data == "tắt máy"||data == "Tắt máy"||data == "10") {digitalWrite(led1, LOW);} data="";} Code tránh vật cản: SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 55 Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức #include #define trig //chân trig HC-SR04 #define echo 4//chân echo HC-SR04 Servo srf05; // create servo object to control a servo #define inA1 //Định nghĩa chân in1 động A #define inA2 //Định nghĩa chân in2 động A #define inB1 //Định nghĩa chân in1 động B #define inB2 //Định nghĩa chân in2 động B void setup() { pinMode(inA1, OUTPUT);//Set chân in1 dc A output pinMode(inA2, OUTPUT);//Set chân in2 dc A output pinMode(inB1, OUTPUT);//Set chân in1 dc B output pinMode(inB2, OUTPUT);//Set chân in2 dc B output pinMode(trig,OUTPUT);//chân trig phát tín hiệu pinMode(echo,INPUT);//chân echo nhận tín hiệu Serial.begin(9600); srf05.attach(5); // attaches the servo on pin to the servo object } void loop() { objectAvoider (inA1, inA2, inB1, inB2,50, 1000); } int objectDistance_cm (byte angle) { srf05.write(angle); delay(500); SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 56 Đồ án tốt nghiệp GVHD: Th.S Hoàng Anh Đức unsigned long duration;//biến đo thời gian int distance;//biến lưu khoảng cách /* phát xung từ chân trig */ digitalWrite(trig,0);//tắt chân trig delayMicroseconds(2); digitalWrite(trig,1);// phát xung từ chân trig delayMicroseconds(5);// xung có độ dài microSeconds digitalWrite(trig,0);//tắt chân trig /* tính tốn thời gian */ duration = pulseIn(echo,HIGH);//đo độ rộng xung HIGH chân echo ( http://arduino.vn/reference/pulsein ) distance = int(duration/2/29.412);//tính khoảng cách đến vật /* in kết Serial monitor */ //Serial.print(distance); //Serial.println("cm"); //delay(200); return distance; } void robotMover (byte inR1, byte inR2, byte inL1, byte inL2, byte action) { /* inR1 inR2 chân tín hiệu động bên phải inL1 inL2 chân tín hiệu động bên trái action= đứng yên action =1 thẳng action =2 lùi lại action =3 quay trái action =4 quay phải SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 57 Đồ án tốt nghiệp action =5 rẽ trái GVHD: Th.S Hoàng Anh Đức action =6 rẽ phải action =7 rẽ lùi trái action =8 rẽ lùi phải */ switch (action) { case 0:// không di chuyển motorControlNoSpeed(inR1, inR2, 0); motorControlNoSpeed(inL1, inL2, 0); break; case 1://đi thẳng motorControlNoSpeed(inR1, inR2, 1); motorControlNoSpeed(inL1, inL2, 1); break; case 2:// lùi lại motorControlNoSpeed(inR1, inR2, 2); motorControlNoSpeed(inL1, inL2, 2); break; case 3:// quay trái motorControlNoSpeed(inR1, inR2, 1); motorControlNoSpeed(inL1, inL2, 2); break; case 4:// quay phải motorControlNoSpeed(inR1, inR2, 2); motorControlNoSpeed(inL1, inL2, 1); break; case 5:// rẽ trái motorControlNoSpeed(inR1, inR2, 1); SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 58 Đồ án tốt nghiệp motorControlNoSpeed(inL1, inL2, 0); GVHD: Th.S Hoàng Anh Đức break; case 6:// rẽ phải motorControlNoSpeed(inR1, inR2, 0); motorControlNoSpeed(inL1, inL2, 1); break; case 7:// rẽ lùi trái motorControlNoSpeed(inR1, inR2, 2); motorControlNoSpeed(inL1, inL2, 0); break; case 8:// rẽ lùi phải motorControlNoSpeed(inR1, inR2, 0); motorControlNoSpeed(inL1, inL2, 2); break; default: action = 0; } } void motorControlNoSpeed (byte in1,byte in2, byte direct) { // in1 and in2 are signal pins to control the motor // en is the enable pin // the defauspeed is the highest // direct includes: // 0:Stop // 1:Move on direct // 2:Move on another direct switch (direct) { SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 59 Đồ án tốt nghiệp case 0:// Dừng không quay GVHD: Th.S Hoàng Anh Đức digitalWrite(in1,LOW); digitalWrite(in2,LOW); break; case 1:// Quay chiều thứ digitalWrite(in1,HIGH); digitalWrite(in2,LOW); break; case 2:// Quay chiều thứ digitalWrite(in1,LOW); digitalWrite(in2,HIGH); break; //default: } } void objectAvoider (byte inR1, byte inR2, byte inL1, byte inL2, byte allow_distance, int turn_back_time) { robotMover(inR1,inR2,inL1,inL2,1); Serial.println("Tiến"); //delay(10); int front_distance=objectDistance_cm (90); int left_distance; int right_distance; int max_distance; if (front_distance > allow_distance) { robotMover(inR1,inR2,inL1,inL2,1); Serial.println("Tiến"); SVTH: Nguyễn Hữu Tuấn Sibunhương Settha 60 Đồ án tốt nghiệp delay(10); GVHD: Th.S Hoàng Anh Đức } if (front_distance