Đồ an tot nghiep robot tu hanh tranh vat can

Đồ án tốt nghiệp điện tử TRƯỜNG ĐHKTCN CỘNG HỒ XÃ HỘI CHỦ NGHĨA VIỆT NAM KHOA CƠ KHÍ Độc lập - Tự - Hạnh phúc NHIỆM VỤ THIẾT KẾ TỐT NGHIỆP Sinh viên thực hiện: Nguyễn Thị Quyên K145520114043 Ngành học: Kỹ thuật Cơ điện tử Lớp: K50 CDT.01 Giáo viên hướng dẫn: ThS Lê Thị Thu Thủy ThS Nguyễn Hữu Chinh Tên đề tài tốt nghiệp: Thiết kế điều khiển robot tự hành tránh vật cản Thông số kỹ thuật yêu cầu: - Robot vận hành linh hoạt có khả tự tránh vật cản - Hệ thống vận hành đơn giản, dễ triển khai Nội dung phần thuyết minh tính tốn : - Tổng quan - Tổng hợp cấu trúc hệ thống - Thiết kế lựa chọn cấu chấp hành - Tích hợp trang bị điện, điện tử Kết đạt được: - Bản vẽ thiết kế - Bản thuyết minh - Sản phẩm thực Ngày giao nhiệm vụ Ngày hoàn thành nhiệm vụ GIÁO VIÊN HƯỚNG DẪN TRƯỞNG BỘ MÔN TRƯỞNG KHOA (Ký ghi rõ họ tên) (Ký ghi rõ họ tên) (Ký ghi rõ họ tên) TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN Đồ án tốt nghiệp điện tử NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… …………………………………………………………………………………………………………… TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN Đồ án tốt nghiệp điện tử MỤC LỤC NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN LỜI CẢM ƠN LỜI NÓI ĐẦU CHƯƠNG TỔNG QUAN VỀ ROBOT TỰ HÀNH TRÁNH VẬT CẢN 1.1 Tổng quan Robot tự hành tránh vật cản 1.1.1 Tình hình phát triển 1.1.2 u cầu cơng nghệ nhu cầu thực tế 1.1.3 Ưu, nhược điểm robot tự hành tránh vật cản 1.1.4 Ứng dụng robot tự hành 1.2 Thông số kỹ thuật cần đạt 1.3 Thơng số kỹ thuật mơ hình chức 1.4 Kết luận CHƯƠNG THIẾT KẾ HỆ THỐNG CƠ KHÍ 2.1 Thiết kế sơ hệ thống khí lựa chọn truyền khí 2.1.1 Tính tốn động 2.1.2 Mơ hình đơn giản xe tự hành 2.1.3 Tính chọn cơng suất động 2.1.4 Chọn thiết bị kèm 2.2 Thiết kế chi tiết khí 2.3 Kết Luận TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN Đồ án tốt nghiệp điện tử CHƯƠNG THIẾT KẾ HỆ THỐNG ĐIỆN, ĐIỆN TỬ 3.1 Chọn phương án thiết kế 3.2 Lựa chọn linh kiện 3.3 Sơ đồ nguyên lý 3.4 Kết luận CHƯƠNG THI CÔNG VÀ ĐÁNH GIÁ SẢN PHẨM 4.1 Thi cơng sản phẩm 4.2 Sản phẩm hồn chỉnh ĐÁNH GIÁ SẢN PHẨM21 DANH MỤC BẢNG BIỂU Bảng 1.1 Thông số kỹ thuật khách hàng đưa Bảng 1.2 Thông số kỹ thuật đặt Bảng 2.1 Tổng hợp nguyên vật liệu khí Bảng 3.1 Tổng hợp nguyên vật liệu điện, điện tử TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN Đồ án tốt nghiệp điện tử DANH MỤC HÌNH VẼ Hình 1.1 Robot tự hành nhà máy Hình 1.2 Robot tự hành siêu thị Hình 1.3 Robot tự hành Gita tự chạy theo chủ nhân Hình 1.4 Robot mang theo thiết bị lĩnh vực khoa học vũ trụ Hình 1.5 Hình ảnh minh họa robot đưa thức ăn Hình 2.1 Mơ hình xe tự hành Hình 2.2 Động RC Servo Hình 2.3 Bánh xe Hình 2.4 Tấm thân xe Hình 3.1 Sơ đồ khối Hình 3.2 Hình ảnh mạch vi xử lý Arduino uno R3 Hình 3.3 Cảm biến siêu âm SRF05 Hình 3.4 Mạch cầu L293N Hình 3.5 Pin 9v Hình 3.6 Sơ đồ nguyên lý Hình 4.1 Hình ảnh tổng hợp chi tiết linh kiện Hình 4.2 Hình sản phẩm hồn chỉnh TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN Đồ án tốt nghiệp điện tử LƠI CAM ƠN Đầu tiên chúng em xin chân thành cảm ơn quý thầy, cô Khoa Điện tử, Trường Đại học Kỹ thuật Công Nghiệp – Đại học Thái Nguyên tận tình truyền đạt kiến thức suốt thời gian học tập trường Với vốn kiến thức quý báu tiếp thu, chúng em có tảng vững để thực tốt đồ án.Và đặc biệt xin gửi lời biết ơn sâu sắc đến thầyPhạm Thành Long tận tình hướng dẫn, đồng thời động viên thời gian tơi nghiên cứu hồn thành đồ án Trong thời gian thực đồ án, có nhiều cố gắng chắn đồ án khơng tránh khỏi thiếu sót Kính mong thầy tận tình bảo góp ý kiến để đồ án hoàn thiện Cuối cùng, chúng em xin kính chúc q Thầy, Cơ dồi sức khỏe, thànhcông nghiệp cao quý Chúng em xin chân thành cảm ơn! TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 1 Đồ án tốt nghiệp điện tử CHƯƠNG TỔNG QUAN VỀ ROBOT TỰ HÀNH TRÁNH VẬT CẢN 1.1 Tổng quan Robot tự hành tránh vật cản 1.1.1 Tình hình phát triển Ngày nay, Robot học đạt thành tựu to lớn sản xuất công nghiệp Những cánh tay robot có khả làm việc với tốc độ cao, xác liên tục làm suất lao động tăng nhiều lần Chúng làm việc môi trường độc hại hàn, phun sơn, nhà máy hạt nhân, hay lắp ráp linh kiện điện tử tạo điện thoại, máy tính…một cơng việc địi hỏi tỉ mỉ, xác cao Tuy nhiên robot có hạn chế chung hạn chế không gian làm việc Không gian làm việc chúng bị giới hạn số bậc tự tay máy vị trí gắn chúng Ngược lại, Robot tự hành lại có khả hoạt động cách linh hoạt môi trường khác Robot tự hành tránh vật cản có khả tự hoạt động, thực thi nhiệm vụ mà không cần can thiệp người Với cảm biến, chúng có khả nhận biết mơi trường xung quanh Robot tự hành tránh vật cản ngày có nhiều ý nghĩa ngành công nghiệp, thương mại, y tế, ứng dụng khoa học phục vụ đời sống người Với phát triển ngành Robot học, robot tự hành tránh vật cản ngày có khả hoạt động mơi trường khác nhau, tùy lĩnh vực áp dụng mà chúng có nhiều loại khác robot sơn, robot hàn, robot cắt cỏ, robot thám hiểm đại dương, robot làm việc vũ trụ Cùng với phát triển yêu cầu thực tế, robot tự hành tránh vật cản tiếp tục đưa thách thức cho nhà nghiên cứu Vấn đề robot tự hành tránh vật cản làm để robot tự hành tránh vật cản hoạt động, nhận biết môi trường thực thi nhiệm vụ đề Vấn đề di chuyển, Robot tự hành tránh vật cản nên di chuyển cấu di chuyển lựa chọn tốt Điều hướng vấn đề nghiên cứu chế tạo Robot tự hành tránh vật cản Trong hiệp hội nghiên cứu Robot tự hành tránh vật cản có hướng nghiên cứu khác nhau: - Hướng thứ nghiên cứu Robot tự hành tránh vật cản có khả điều hướng tốc độ cao nhờ thông tin thu từ cảm biến, loại robot có khả hoạt động mối trường phịng mơi trường bên ngồi Loại robot u cầu khả tính tốn đồ sộ trang bị cảm biến có độ nhạy cao, dải đo lớn để điều khiển robot di chuyển tốc độ cao, môi trường có địa hình phức tạp - Hướng thứ hai: nhằm giải vấn đề loại robot tự hành tránh vật cản dùng để hoạt động mơi trường phịng Loại robot tự hành tránh vật cản có kết cấu đơn giản loại trên, thực nhiệm vụ đơn giản TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 2 Đồ án tốt nghiệp điện tử Bài toán dẫn hướng cho robot tự hành tránh vật cản chia làm loại: tốn tồn cục (global) tốn cục (local) Ở tồn cục, mơi trường làm việc robot hoàn toàn xác định, đường vật cản hồn tồn biết trước Ở tốn cục bộ, môi trường hoạt động robot chưa biết trước biết phần Các cảm biến thiết bị định vị cho phép robot xác định vật cản, vị trí mơi trường giúp tới mục tiêu Các vấn đề gặp phải điều hướng cho Robot tự hành tránh vật cản thường khơng giống loại robot khác Để điều hướng cho Robot tránh vật cản , định theo thời gian thực phải dựa vào thông tin liên tục môi trường thông qua cảm biến, mơi trường phịng ngồi trời, điểm khác biệt lớn so với kỹ thuật lập kế hoạch ngoại tuyến Robot tự hành tránh vật cản phải có khả tự định phương thức điều hướng, định hướng chuyển động để tới đích thực nhiệm vụ định Điều hướng cho robot tự hành tránh vật cản công việc đòi hỏi phải thực số khả khác nhau, bao gồm : khả di chuyển mức bản, ví dụ hoạt động tới vị trí cho trước; khả phản ứng kiện theo thời gian thực, ví dụ có xuất đột ngột vật cản; khả xây dựng, sử dụng trì đồ mơi trường hoạt động; khả xác định vị trí robot đồ đó; khả thiết lập kế hoạch để tới đích tránh tình khơng mong muốn khả thích nghi với thay đổi môi trường hoạt động 1.1.2 Yêu cầu công nghệ nhu cầu thực tế Trong nhà xưởng, người cơng nhân bị gị bó suất lao động, robot tự hành tránh vật cản giải pháp nhằm cung cấp nguyên liệu từ nhà kho đến tay người công nhân, giúp giảm nhân cơng, tăng suất Tùy theo loại ngun liệu có đặc thù, khối lượng, kích thước khác mà thiết kế người ta vào đưa yêu cầu : - Kích thước - Khối lượng - Công suất - Tốc độ di chuyển Trong đồ án chúng em chủ yếu tập trung nghiên cứu thiết kế modul chuyển động điều khiển robot chở nguyên liệu dây chuyền sản xuất, với kích thước thơng số nhỏ thực tế Kết nhóm robot tự động có khả di chuyển tự mơi trường có vật cản Đề tài thiết kế chế tạo robot tự hành tránh vật cản chủ yếu giúp cho sinh viên làm quen với việc sử dụng vi điều khiển, kĩ lập trình, phương pháp tự thập liệu từ cảm biến sensor, điều khiển động cơ, hiển thị trạng thái hoạt động hệ thống mạch phụ trợ khác TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 3 Đồ án tốt nghiệp điện tử 1.1.3 Ưu, nhược điểm robot tự hành tránh vật cản  Ưu điểm - Không gây nhiễm, sử dụng bình điện, sạc - Sử dụng không gian hiệu quả, đa nhà máy xí nghiệp - Giá thành khơng đắt  Nhược điểm - Không thể leo bậc thang có chiều cao q 1/2 bán kính bánh xe - Thiếu tính linh hoạt thích ứng làm việc vị trí khác 1.1.4 Ứng dụng robot tự hành - Robot làm việc môi trường độc hại - Ứng dụng robot tự hành nhà máy Hình 1.1 Robot tự hành nhà máy - Ứng dụng robot tự hành siêu thị TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 4 Đồ án tốt nghiệp điện tử Hình 1.2 Robot tự hành siêu thị - Ứng dụng robot tự hành lĩnh vực sống Hình 1.3 Robot tự hành Gita tự chạy theo chủ nhân - Ứng dụng robot tự hành lĩnh vực khoa học vũ trụ Hình 1.4 Robot mang theo thiết bị lĩnh vực khoa học vũ trụ - Robot di chuyển theo theo vạch mầu thiết kế sẵn nhà xưởng đển vị trí yêu cầu thực thao tác TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 5 Đồ án tốt nghiệp điện tử  Thiết kế sơ phần khí gồm: Thân xe, bánh chính, động Bảng 2.1 Tổng hợp nguyên vật liệu khí STT Tên chi tiết Động RC servo Ký hiệu Thông số Số lượng Mua / Chế tạo - Điện áp hoạt động: 4.8-5VDC -Tốc độ: 0.12 sec/ 60 degrees (4.8VDC) -Lực kéo: 1.6KG.CM -Kích thước: 21x12x22mm -Trọng lượng: 9g Mua Bánh xe - Đường kính bánh xe: 95mm - Cỡ trục: 6mm - Bề rộng: 14mm - Chất liệu: Viền cao su ma sát tốt - Khối lượng tải : 5kg Mua Tấm thân xe - Đường kính: 300mm - Chất liệu: Mika 5mm Chế tạo Bu lông Mua TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 12 Đồ án tốt nghiệp điện tử Đai ốc Mua 2.3 Kết Luận Các chi tiết cấu khí thiết kế theo hình dạng kích thước, phù hợp với yêu cầu kỹ thuật Sau hoàn thành phần thiết kế khí ta chuyển sang thiết kế hệ thống điện, điện tử TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 13 Đồ án tốt nghiệp điện tử CHƯƠNG THIẾT KẾ HỆ THỐNG ĐIỆN, ĐIỆN TỬ 3.1 Chọn phương án thiết kế Bo mạch chủ dùng arduino để tiếp nhận tín hiệu từ sensor phát lệnh điều khiển hoạt động xe Hình 3.1 Sơ đồ khối  Chức khối:  Cảm biến (sensor) cảm nhận đại lượng điện không điện, chuyển đổi chúng trở thành tín hiệu điện Phù hợp với thiết bị thu nhận tín hiệu  Bộ xử lý ( processer) nhận tín hiệu từ cảm biến, sau xử lý chuyển đổi tín hiệu đưa đến modul kích chuyển động 3.2 Lựa chọn linh kiện  Mạch arduino uno R3 Arduino board mạch vi xử lý, nhằm xây dựng ứng dụng tương tác với với môi trường thuận lợi Phần cứng bao gồm board mạch nguồn mở thiết kế tảng vi xử lý AVR Atmel 8bit, ARM Atmel 32-bit Những Model trang bị gồm cổng giao tiếp USB, chân đầu vào analog, 14 chân I/O kỹ thuật số tương thích với nhiều board mở rộng khác TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 14 Đồ án tốt nghiệp điện tử Hình 3.2 Hình ảnh mạch vi xử lý Arduino uno R3 Thông số kỹ thuật: Microcontroller(Chip sử dụng) : ATmega328 Operating Voltage: 5V Input Voltage (recommended): 7-12V Input Voltage (limits): 6-20V Digital I/O Pins: 14 (of which provide PWM output) Analog Input Pins: DC Current per I/O Pin: 40 mA DC Current for 3.3V Pin : 50 mA Flash Memory: 32 KB (ATmega328) of which 0.5 KB used by bootloader SRAM: KB (ATmega328) EEPROM: KB (ATmega328) Clock Speed: 16 MHzXung nhịp: 16MHz  Module Cảm Biến Siêu Âm SRF05 - chân: VCC / Trige / Echo / OUT / GND - Điện áp hoạt động : +5V DC - Khoảng cách: 2cm - 450cm - Không nên đo khoảng cách 0,3 cm Hình 3.3 Cảm biến siêu âm SRF05  Mạch cầu H - Nhiệm vụ: Điều khiển tốc độ quay, hướng quay động -Thông số kỹ thuật: TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 15 Đồ án tốt nghiệp điện tử Arduino Motor Shield L293D phần board mở rộng cho board arduino, dùng để điều khiển loại động DC, động bước động servo Arduino Motor Shield thiết kế gọn gàng, đẹp mắt tương thích hồn tồn với board Arduino: arduino uno r3, arduino leonardo, arduino mega2560, giúp bạn sử dụng điều khiển cách dễ dàng nhanh chóng Arduino Motor Shield sử dụng IC cầu H L293D hoàn chỉnh với chế độ bảo vệ IC logic 74HC595 để điều khiển động Arduino Motor Shield L293D điều khiển nhiều loại motor khác step motor, servo motor, motor DC, với mức áp lên đến 36V, dòng tối đa 600mA cho kênh điều khiển Hình 3.4 Mạch cầu L293D  L293D gồm chân: Chân điều khiển RC servo kết nối với chân số 10 Nguồn cung cấp lấy trực tiếp từ board Arduino      Motor nối với chân 11 Motor nối với chân Motor nối với chân Motor nối với chân Chân 4, 7, 8, 12 dùng điều khiển motor thông qua IC 74HC595 TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 16 Đồ án tốt nghiệp điện tử Ngoài để tiện cho việc sử dụng nguồn cắm ngoài, Arduino Motor Shield sử dụng jumper PWR mục đích để lấy nguồn ngồi thơng qua jack DC board arduino để cung cấp nguồn cho motor hoạt động Nếu trường hợp không sử dụng jumper thỉ phải cấp nguồn riêng vào chân EXT_PWR để cấp nguồn cho motor hoạt động  Nguồn 9v Hình 3.5 Pin 9v Bảng 3.1 Tổng hợp nguyên vật liệu điện, điện tử STT Tên linh kiện Ký hiệu Thông số Số lượng Mua / Chế tạo Arduino Arduino uno R3 Mua Module Cảm Biến Siêu Âm SRF05 Mua TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 17 Đồ án tốt nghiệp điện tử Sensor MH-Sensor Mua Modul LM2596 LM2596 Mua Pin 3.7v Mua 3.3 Sơ đồ nguyên lý Hình 3.6 Sơ đồ nguyên lý 3.4 Kết luận Các modul thiết kế, hoạt hoàn thành theo động ổn định TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 18 Đồ án tốt nghiệp điện tử theo thông số đề Sau hồn thành phần khí điện, điện tử, ta tiến hành thi công lắp ráp sản phẩm TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 19 Đồ án tốt nghiệp điện tử CHƯƠNG THI CÔNG VÀ ĐÁNH GIÁ SẢN PHẨM 4.1 Thi cơng sản phẩm Hình 4.1 Hình ảnh tổng hợp chi tiết linh kiện 4.2 Sản phẩm hồn chỉnh Hình 4.2 Sản phẩm hồn chỉnh TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 20 Đồ án tốt nghiệp điện tử ĐÁNH GIÁ SẢN PHẨM  Q trình thực hồn thành nhiệm vụ sau: - Thiết kế khung khí hồn chỉnh - Thiết kế điện, điện tử, mơ thi công mạch chạy ổn định - Tự hành tự tránh vật cản đường  Tuy nhiên mạch số hạn chế như: - Tốc độ chưa khống chế cách hợp lý - Cách bố trí mạch chưa khoa học  Một số cách khắc phục: - Tăng số lượng sensor đầu dị - Chất liệu làm bánh xe có độ bám tốt TÀI LIỆU THAM KHẢO [1] www.banlinhkien.vn [2] www.codientu.org [3] http://arduino.vn/ [4] http://hshop.vn/ [5] http://www.3dcontentcentral.com/ [6] TS Phạm Thành Long, Bài giảng Robot công nghiệp, Thái Nguyên 2014 [7] GS.TSKH Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất khoa học kỹ thuật Hà Nội 2006 TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 21 Đồ án tốt nghiệp điện tử PHỤ LỤC #include //Includes I2C Lib #include #include #define #define #define #define #define ECHO_PIN 12 //chân ECHO siêu âm - nối 12 Shield V5.0 TRIG_PIN 13 // chân ECHO siêu âm - nối 13 Shield V5.0 MAX_DISTANCE 200 MAX_SPEED 235 // sets speed of DC motors MAX_SPEED_OFFSET 20 Servo myservo; NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); boolean goesForward=false; int distance = 100; int speedSet = 0; int dugme=1; // Nối chân L298N với const int enAPin = 6; const int in1Pin = 7; const int in2Pin = 5; const int in3Pin = 4; const int in4Pin = 2; const int enBPin = 3; int command; int speedCar = 220; int speed_Coeff = 4; shield V5.0 // (V5) chân // (V5) chân // (V5) chân // (V5) chân // (V5) chân // (V5) chân - ENA IN1 IN2 IN3 IN4 ENB (L298N) (L298N) (L298N) (L298N) (L298N) (L298N //bien luu trang thai Bluetooth // toc 50 - 255 // he so suy giam giam may lan toc void setup() { pinMode(enAPin, pinMode(in1Pin, pinMode(in2Pin, pinMode(in3Pin, pinMode(in4Pin, pinMode(enBPin, OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); Serial.begin(9600); //Init Serial port rate set to 9600 myservo.attach(11); //servo nối chân số 11 Shield V5.0 myservo.write(95); //Góc ban đầu Servo, lệch (không thẳng) bạn chỉnh giá trị khác tăng giảm delay(2000); distance = readPing(); delay(100); distance = readPing(); delay(100); distance = readPing(); delay(100); TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 22 Đồ án tốt nghiệp điện tử distance = readPing(); delay(100); } void loop() { long duration, distance; int distanceR = 0; int distanceL = 0; while(dugme==1) { distance = readPing(); delay(40); if(distance=distanceL) { goRight(); delay(300); stopRobot(); delay(100); }else { goLeft(); delay(300); stopRobot(); delay(100); } }else { goAhead(); } } } void goAhead(){ digitalWrite(in1Pin,LOW); digitalWrite(in2Pin,HIGH); digitalWrite(in3Pin,LOW); digitalWrite(in4Pin,HIGH); // LEFT tien // RIGHT tien TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 23 Đồ án tốt nghiệp điện tử analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar); } void goBack(){ digitalWrite(in1Pin,HIGH); digitalWrite(in2Pin,LOW); digitalWrite(in3Pin,HIGH); digitalWrite(in4Pin,LOW); analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar); } // LEFT lui // RIGHT lui void goRight(){ digitalWrite(in1Pin,LOW); digitalWrite(in2Pin,HIGH); digitalWrite(in3Pin,HIGH); digitalWrite(in4Pin,LOW); analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar); // LEFT tien // RIGHT lui } void goLeft(){ digitalWrite(in1Pin,HIGH); digitalWrite(in2Pin,LOW); digitalWrite(in3Pin,LOW); digitalWrite(in4Pin,HIGH); analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar); // LEFT lui // RIGHT tien } void goAheadRight(){ //vua tien - vua quay phai digitalWrite(in1Pin,LOW); digitalWrite(in2Pin,HIGH); // LEFT tien digitalWrite(in3Pin,LOW); digitalWrite(in4Pin,HIGH); // RIGHT tien analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar/speed_Coeff); //banh phai quay cham hon } void goAheadLeft(){ digitalWrite(in1Pin,LOW); digitalWrite(in2Pin,HIGH); // LEFT tien digitalWrite(in3Pin,LOW); digitalWrite(in4Pin,HIGH); // RIGHT tien analogWrite(enAPin, speedCar/speed_Coeff); //banh trai quay cham hon analogWrite(enBPin, speedCar); TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 24 Đồ án tốt nghiệp điện tử } void goBackRight(){ //vua lui - vua quay phai digitalWrite(in1Pin,HIGH); digitalWrite(in2Pin,LOW); // LEFT lui digitalWrite(in3Pin,HIGH); digitalWrite(in4Pin,LOW); // RIGHT lui analogWrite(enAPin, speedCar); analogWrite(enBPin, speedCar/speed_Coeff); //banh phai quay cham hon } void goBackLeft(){ digitalWrite(in1Pin,HIGH); digitalWrite(in2Pin,LOW); // LEFT lui digitalWrite(in3Pin,HIGH); digitalWrite(in4Pin,LOW); // RIGHT lui analogWrite(enAPin, speedCar/speed_Coeff);//banh trai quay cham hon analogWrite(enBPin, speedCar); } void stopRobot(){ digitalWrite(in1Pin,LOW); digitalWrite(in2Pin,LOW); digitalWrite(in3Pin,LOW); digitalWrite(in4Pin,LOW); analogWrite(enAPin, 0); analogWrite(enBPin, 0); } int lookRight() { myservo.write(40); delay(500); int distance = readPing(); delay(100); myservo.write(95); return distance; } int lookLeft() { myservo.write(150); delay(500); int distance = readPing(); delay(100); myservo.write(95); return distance; delay(100); } int readPing() { delay(70); int cm = sonar.ping_cm(); //cái chỉnh cho thẳng //cái chỉnh cho thẳng TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 25 Đồ án tốt nghiệp điện tử if(cm==0) { cm = 250; } return cm; } TRƯỜNG ĐẠI HỌC KỸ THUẬT CÔNG NGHIỆP THÁI NGUYÊN 26