Đồ án tốt nghiệp kỹ sư cơ điện tử trường đại học bách khoa hà nội năm 2021Các yêu cầu thiết kế hệ thống cơ khí cần thỏa mãn các điều kiện sau: Kích thước: 360 x 500 x 840 (mm) Xe đảm bảo độ cứng vững An toàn khi vận hành Thiết kế thuận tiện cho tháo lắp sửa chữa, bố trí bộ điều khiển, các bộphận khác Vỏ xe được thiết kế bao toàn bộ các cơ cấu cơ khí bên trong Tính năng: leo được cầu thang thông thường (độ cao bậc thang từ1518cm, chiều rộng bậc thang 2530cm, độ dốc 3336°)
TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ BỘ MÔN CƠ ĐIỆN TỬ ĐỒ ÁN TỐT NGHIỆP THIẾT KẾ, CHẾ TẠO ROBOT LEO CẦU THANG DÙNG KHUNG BÁN NGUYỆT NGUYỄN VĂN AN an.nv164932@sis.hust.edu.vn NƠNG MINH HỊA hoa.nm164928@sis.hust.edu.vn TẠ XUÂN LỘC loc.tx162558@ sis.hust.edu.vn TẠ ĐÌNH THĂNG Thang.td163807@sis.hust.edu.vn Chuyên ngành Cơ điện tử Giảng viên hướng dẫn: Bộ môn: Viện: TS Bùi Đình Bá Cơ điện tử Cơ khí HÀ NỘI, 1/2020 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH LỜI CẢM ƠN Trong thời gian làm đồ án tốt nghiệp, nhóm chúng em nhận nhiều giúp đỡ, đóng góp ý kiến bảo nhiệt tình thầy cơ, gia đình bạn bè Em xin gửi lời cảm ơn chân thành đến TS.BÙI ĐÌNH BÁ, giảng viên Bộ môn Cơ Điện tử, viện Cơ Khí, trường Đại học Bách Khoa Hà Nội - người tận tình hướng dẫn, bảo em suốt trình làm đồ án Chúng em xin chân thành cảm ơn TS NGUYỄN THÀNH HÙNG có ý kiến, bảo, giúp đỡ tận tình giúp chúng em hoàn thiện đồ án tốt nghiệp Em xin chân thành cảm ơn thầy cô giáo viện Cơ Khí nói chung, thầy Bộ mơn Cơ điện tử nói riêng dạy dỗ cho em kiến thức môn đại cương mơn chun ngành, giúp em có sở lý thuyết vững vàng tạo điều kiện giúp đỡ em suốt q trình học tập hồn thành đồ án Với điều kiện thời gian kiến thức kinh nghiệm hạn chế, báo cáo đồ án tránh thiếu sót Chúng em mong nhận bảo, đóng góp ý kiến thầy để đồ án chúng em hoàn thiện Chúng em xin chân thành cảm ơn! ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH TĨM TẮT NỘI DUNG ĐỒ ÁN Trong suốt q trình phát triển, lồi người ln tạo công cụ lao động để nâng cao suất lao động, cải thiện chất lượng sống, thay người thực công việc nặng nhọc, nguy hiểm Từ mục đích nhóm chúng em đưa ý tưởng thiết kế robot leo cầu thang để thay người mang vác, vận chuyện hàng hóa tịa nhà chưa có thang máy Sau tìm hiểu số mơ hình robot leo cầu thang có thực tế, với mong muốn thiết kế mơ hình robot leo cầu thang chưa có trên thị trường hướng tới sáng tạo đa dạng hóa mơ hình robot leo cầu thang nhóm chúng em định thiết kế mơ hình “Robot leo cầu thang dùng khung bán nguyệt” sử dụng hai bánh kết hợp hai trợ lực để leo cầu thang Với mục tiêu đó, chúng em vận dụng kiến thức động học động lực học để xác định thông số cần thiết cho robot Từ lựa chọn thiết bị phù hợp Nhóm chúng em sử dụng phần mềm Solidworks AutoCad để thiết kế mơ hình robot Do thời gian điều kiện chưa cho phép, nhóm chúng em xây dựng mơ hình in 3D với vật liệu nhựa, kết hợp với cảm biến vật cản, động servo MG996, module bluetooth HC05… hệ thống điều khiển sử dụng Arduino nano điều khiển qua app điện thoại androi để thể tính khả thi mơ hình Từ kết thực tế mơ hình cho thấy thiết kế thực nhiệm vụ đặt ban đầu robot có khả leo cầu thang Tuy kết thực tế đồ án đạt phần, đề tài có tính ứng dụng thực tế cao đời sống Hi vọng sau đồ án này, nhóm có nghiên cứu phát triển thêm để chế tạo mơ hình thực tế đạt mục tiêu đưa Qua đồ án này, chúng em học nhiều kiến thức việc sử dụng thiết bị điện tử động Ngồi ra, cịn trau dồi cho thân kỹ văn phịng lập trình Sinh viên thực Nguyễn Văn An Nơng Minh Hịa Tạ Xn Lộc Tạ Đình Thăng ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH MỤC LỤC CHƯƠNG 1.1 TỔNG QUAN VỀ CÁC LOẠI ROBOT Giới thiệu chung 1.1.1 Khái niệm Robot 1.1.2 Xu hướng phát triển 1.2 Tổng quan lịch phát triển mobile Robot 1.3 Phân loại mobile Robot 1.3.1 Phân loại theo phương pháp di chuyển 1.3.2 Phân loại theo môi trường hoạt động 1.3.3 Phân loại theo phương pháp điều khiển 1.4 Ứng dụng robot vượt địa hình 1.5 Một số loại robot vượt địa hình giới 1.6 Ý tưởng thiết kế CHƯƠNG THIẾT KẾ HỆ THỐNG CƠ KHÍ 11 12 2.1 Yêu cầu thiết kế 12 2.2 Động học robot leo cầu thang 13 2.2.1 Khái niệm toán đặt 13 2.2.2 Phương trình động học robot 14 2.2.3 Động lực học robot 15 2.3 2.4 2.5 Chọn động 22 2.3.1 Chọn động trước sau 22 2.3.2 Tính chọn động dẫn động 25 Tính tốn kiểm nghiệm trục 26 2.4.1 Kiểm nghiệm trục độ bền mỏi 27 2.4.2 Giới hạn mỏi xoắn 27 2.4.3 Kiểm nghiệm trục độ bền tĩnh 28 Chọn cấu truyền động 29 2.5.1 Khái niệm 30 2.5.2 Chọn loại xích 30 2.5.3 Xác định thơng số xích truyền 32 2.5.4 Khoảng cách trục số mắt xích 34 2.5.5 Tính kiểm nghiệm xích độ bền 35 ĐỒ ÁN TỐT NGHIỆP BÁ 2.5.6 CHƯƠNG 3.1 3.2 3.3 3.4 GVHD: TS BÙI ĐÌNH Xác định thơng số đĩa xích lực tác dụng lên trục 36 THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 39 Sơ đồ hệ thống điều khiển 39 3.1.1 Sơ đồ khối hệ thống điều khiển 39 3.1.2 Lưu đồ thuật toán điều khiển 40 Lựa chọn linh kiện robot 41 3.2.1 Bo mạch Arduino Nano 41 3.2.2 Pin lipo TCB 12v-3S 42 3.2.3 Module hạ áp LM 2576 43 3.2.4 Module bluetooth HC05 44 3.2.5 Cảm biến vật cản hồng ngoại E3F-DS30C4 45 3.2.6 Động RC servo MG996 46 Phần mềm lập trình điều khiển 47 3.3.1 Phần mềm Arduino IDE 47 3.3.2 Phần mềm tạo app điều khiển MIT app inventor 48 Sơ đồ kết nối CHƯƠNG MƠ HÌNH VÀ KẾT QUẢ THỰC NGHIỆM 50 52 4.1 Mơ hình thực nghiệm 52 4.2 Kết 53 4.2.1 Khả leo cầu thang 53 4.2.2 Hạn chế hướng khắc phục 53 KẾT LUẬN 54 CHƯƠNG 5.1 Kết luận 54 5.2 Hướng phát triển đồ án tương lai 54 TÀI LIỆU THAM KHẢO 55 PHỤ LỤC 56 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH DANH MỤC HÌNH VẼ Hình 1.1 Robot RHex di chuyển sử dụng dạng bánh Hình 1.2 Robot di chuyển sử dụng chân Hình 1.3 Robot Packbot 510 di chuyển sử dụng bánh xích Hình 1.4 Máy bay khơng người lái UAV Hình 1.5 Robot di chuyển nước Hình 1.6 Robot di chuyển mặt đất Hình 1.7 Robot Sojourner bề mặt Sao Hỏa Hình 1.8 Robot vận chuyển hàng hóa nhà máy Hình 1.9 Robot cứu hỏa Thermite Hình 1.10 iRobot 210 Hình 1.11 iRobot’s 710 Warrior Hình 1.12 iRobot's 510 Packbot Hình 1.13 Robot thám hiểm Sao Hỏa Opportunity Hình 2.1 Mơ hình robot Hình 2.2 Ngun lí leo cầu thang robot Hình 2.3 Mơ hình robot Hình 2.4 Mơ hình động học robot Hình 2.5 Các toán chuyển động thực tế robot Hình 2.6 Vận tốc xe đường Hình 2.7 Xe trạng thái lên dốc Hình 2.8 Mơ hình robot Hình 2.9 Động gạt nước Hình 2.10 Các trạng thái hoạt động robot Hình 2.11 Động planet Hình 2.12 Sơ đồ đặt lực biểu đồ momen trục Hình 2.13 Bộ truyền xích Hình 2.14 Cấu tạo xích ống Hình 2.15 Cấu tạo xích ống lăn Hình 2.16 Cấu tạo xích Hình 2.17 Tỉ lệ hệ số ảnh hưởng số đĩa xích số Hình 3.1 Sơ đồ khối hệ thống điều khiển Hình 3.2 Sơ đồ khối hệ thống Hình 3.3 Lưu đồ thuật tốn điều khiển Hình 3.4 Arduino nano Hình 3.5 Pin lipo TCB 12v-3S 4 5 6 9 10 10 11 12 13 14 14 15 16 19 23 24 25 26 26 30 31 31 31 37 39 40 41 42 43 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.6 Modul hạ áp LM 2576 Hình 3.7 Module bluetooth HC05 Hình 3.8 Cảm biến vật cản hồng ngoại E3F-DS30C4 Hình 3.9 Động RC servo MG996 Hình 3.10 Phần mềm lập trình cho Arduino Hình 3.11 Giao diện phần mềm app inventor Hình 3.12 Tạo giao diện cho app điều khiển MIT app inventor Hình 3.13 Lập trình cho app điều khiển Hình 3.14 Giao diện app điều khiển điện thoại Hình 3.15 Bản vẽ sơ đồ điện hệ thống Hình 4.1 Mơ hình thực tế Robot Hình 4.2 Một số góc nhìn Robot Hình 4.3 Một số góc nhìn Robot 44 45 46 47 48 48 49 49 50 51 52 52 53 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH DANH MỤC BẢNG Bảng 2.1 Giá thành tương đối loại ổ lăn Bảng 2.2 Các thông số xích lăn Bảng 2.3 Số tỉ số truyền Bảng 2.4 Cơng suất cho phép xích lăn 29 34 34 36 Bảng 2.5 Trị số bước xích lớn cho phép 𝑃𝑚𝑎𝑥 36 Bảng 2.6 Số lần va đập cho phép 37 loại xích Bảng 2.7 Trị số hệ số an tồn Bảng 2.8 Vật liệu chế tạo đĩa xích phương pháp nhiệt luyện 38 38 ĐỒ ÁN TỐT NGHIỆP BÁ CHƯƠNG GVHD: TS BÙI ĐÌNH TỔNG QUAN VỀ CÁC LOẠI ROBOT 1.1 Giới thiệu chung 1.1.1 Khái niệm Robot Thuật ngữ “Robot” lần xuất năm 1922 tác phẩm “Rosum’s Universal Robot” Karel Capek Theo tiếng Czech Robot người làm tạp dịch Trong tác phẩm nhân vật Rosum trai ông tạo máy gần giống người để hầu hạ người Hơn 20 năm sau, ước mơ viễn tưởng Karel Capek bắt đầu thực Ngay sau chiến tranh giới lần thứ 2, Mỹ xuất tay máy chép hình điều khiển từ xa, phịng thí nghiệm phóng xạ Năm 1959, Devol Engelber chế tạo Robot công nghiệp công ty Unimation Tiếp theo Mỹ, nước khác bắt đầu sản xuất Robot Công Nghiệp: Anh – (1967), Thụy Điển – (1968), CHLB Đức – (1971), Pháp – (1972), Ý – (1973),… Năm 1967, Nhật Bản nhập Robot công nghiệp từ công ty AMF (American Machine and Foundry Company) Mỹ Đến năm 1990 có 40 cơng ty Nhật, có cơng ty khổng lồ Hitachi, Mitsubishi Honda đưa thị trường nhiều loại Robot tiếng Từ năm 70, việc nghiên cứu nâng cao tính robot ý nhiều đến lắp đặt thêm cảm biến ngoại tín hiệu để nhận biết mơi trường làm việc Năm 1967, trường đại học tổng hợp Stanford, người ta tạo loại Robot lắp ráp tự động điều khiển vi tính sở xử lý thông tin từ cảm biến lực thị giác Vào thời gian công ty IBM chế tạo Robot có cảm biến xúc giác cảm biến lực điều khiển máy vi tính để lắp ráp máy in gồm 20 cụm chi tiết Năm 1976, hãng General Motor chế tạo thành công cánh tay robot sử dụng tàu Viking quan hàng không vũ trụ NASA nhằm lấy mẫu đất hỏa Những năm 90 áp dụng rộng rãi tiến khoa học vi xử lý công nghệ thông tin, số lượng Robot công nghiệp tăng nhanh, giá thành giảm rõ rệt, tính có nhiều bước tiến vượt bậc Nhờ vậy, Robot cơng nghiệp có vị trí quan trọng dây truyền sản xuất đại Ngày nay, chuyên ngành khoa học nghiên cứu Robot “Robotics” trở thành lĩnh vực rộng khoa học, bao gồm vấn đề cấu trúc cấu động học, động lực học, lập trình quỹ đạo, cảm biến tín hiệu, điều khiển chuyển động v.v… 1.1.2 Xu hướng phát triển Robot 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 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH 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 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) Robot di động nghiên cứu nhiều xe tự hành mặt đất AGV (Autonomous Guided Vehicles), Robot tự hành nước AUV (Autonomous Underwater Vehicles), Máy bay không người lái UAV (Unmanned Arial Vehicles) 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 Hiện ứng dụng robot có xu chuyển sang ứng dụng thường nhật Robot gia đình (home robots) Robot cá nhân (Personal robots) Mặc dù cấu trúc loại robot có khác nghiên cứu hướng ứng dụng dịch vụ hoạt động robot môi trường tự nhiên Với phát triển xã hội q trình đại hóa nước phát triển nhiều dịch vụ hình thành làm thay đổi quan điểm robot từ robot phục vụ công nghiệp sang robot phục vụ cho nhu cầu xã hội nhu cầu cá nhân người 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 1.2 Tổng quan lịch phát triển mobile Robot Robot vượt địa hình hay cịn biết đến với tên mobile robot hiểu loại máy tự động có khả di chuyển (“A mobile robot is an automatic machine that is capable locomotion” – Wikipedia ) Môi trường hoạt động mobile robot đất, nước, khơng khí, khơng gian vũ trụ tổ hợp chúng Địa hình bề mặt mà robot di chuyển đa dạng, phẳng mấp mơ, gồ ghề Mobile robot từ ngày đầu, trải qua trình phát triển tới ngày có nhiều thay đổi, đặc biệt công nghệ sử dụng chế tạo robot Dưới số mốc lớn lịch sử phát triển robot Từ năm 1939 đến năm 1945: Trong chiến tranh giới thứ II mobile robot xuất hiện, hầu hết chúng bom bay Đó kết thành tựu cơng nghệ có liên quan tới khoa học máy tính điều khiển Từ năm 1948 đến năm 1949: W.Gray Walter chế tạo nên Elmer Elsie, hai robot tự động trang bị cảm biến ánh sáng, chúng phát ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.10 Động RC servo MG996 Thông số kỹ thuật: ▪ Chủng loại: Analog RC Servo ▪ Điện áp hoạt động: 4.8~6.6VDC ▪ Lực kéo: 3.5 kg-cm (180.5 ozin) at 4.8V-1.5A ▪ Tốc độ quay: 0.17sec / 60 degrees (4.8V no load) ▪ Kích thước: 40mm x 20mm x 43mm ▪ Trọng lượng: 55g 3.3 Phần mềm lập trình điều khiển 3.3.1 Phần mềm Arduino IDE Arduino tảng mã nguồn mở sử dụng để xây dựng ứng dụng điện tử tương tác với với môi trường thuận lợi Arduino giống máy tính nhỏ để người dùng lập trình thực dự án điện tử mà khơng cần phải có cơng cụ chuyên biệt để phục vụ việc nạp code Arduino gồm phần cứng gồm board mạch mã nguồn mở (thường gọi vi điều khiển): lập trình phần mềm hỗ trợ phát triển tích hợp Arduino IDE (Arduino Integrated Development Environment) trình soạn thảo văn bản, giúp viết code để nạp vào bo mạch Arduino 54 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.11 Phần mềm lập trình cho Arduino 3.3.2 Phần mềm tạo app điều khiển MIT app inventor MIT app inventor là ứng dụng web nguồn mở ban đầu cung cấp Google trì Viện Cơng nghệ Massachusetts (MIT) Nền tảng cho phép nhà lập trình tạo ứng dụng phần mềm cho hệ điều hành Android (OS) Bằng cách sử dụng giao diện đồ họa, tảng cho phép người dùng kéo thả khối mã (blocks) để tạo ứng dụng chạy thiết bị Android Hình 3.12 Giao diện phần mềm app inventor 55 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.13 Tạo giao diện cho app điều khiển MIT app inventor Hình 3.14 Lập trình cho app điều khiển 56 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.15 Giao diện app điều khiển điện thoại 3.4 Sơ đồ kết nối Các thiết bị kết nối với tạo thành hệ thống điều khiển hoàn chỉnh sau: 57 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 3.16 Bản vẽ sơ đồ điện hệ thống 58 ĐỒ ÁN TỐT NGHIỆP BÁ CHƯƠNG GVHD: TS BÙI ĐÌNH MƠ HÌNH VÀ KẾT QUẢ THỰC NGHIỆM 4.1 Mơ hình thực nghiệm Qua thời gian nghiên cứu, thiết kế, nhóm chúng em thiết kế chế tạo thành cơng mơ hình robot sử dung khung bán nguyệt leo cầu thang Kích thước: 390mm x 200mm x 160mm Bán kính khung bán nguyệt: 150mm Hình 4.1 Mơ hình thực tế Robot 59 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 4.2 Một số góc nhìn Robot 60 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH Hình 4.3 Một số góc nhìn Robot 4.2 Kết Khả leo cầu thang Sau hồn thành mơ hình, nhóm kiểm tra khả leo cầu thang robot cách cho robot di chuyển leo cầu thang với kích thước khác Kết cho thấy robot có khả leo cầu thang với độ cao tối đa 10cm, chiều rộng bậc thang 25cm với tốc xe: 0.1 m/s độ dốc 25° 61 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH CHƯƠNG KẾT LUẬN 5.1 Kết luận Qua thời gian nghiên cứu thực hiện, đến nhóm chúng em hồn thành đồ án tốt nghiệp với đề tài: "Thiết kế chế tạo robot leo cầu thang dùng khung bán nguyệt " giảng viên TS Bùi Đình Bá hướng dẫn Đồ án đạt kết thiết kế mơ hình mini sử dụng arduino điều khiển app điện thoại chứng tính khả thi ý tưởng, tính tốn chọn động cơ, truyền xích, đường kính trục, xây dựng giải tốn động học, động lực học mobile robot Thơng qua đồ án chúng em trau dồi thêm nhiều kiến thức mới; vận dụng kiến thức lý thuyết học, áp dụng vào thiết kế hệ thống điện tử thực tế từ khâu thiết kế, lắp ráp cấu khí, điều khiển lập trình Ngồi chúng em cịn học thêm nhiều kỹ như: kỹ tự tìm kiếm tài liệu, tổng hợp thơng tin, kỹ chế bản, kỹ trình bày,… Giúp chúng em có thêm kiến thức kinh nghiệm cần thiết để áp dụng vào công việc thực tế sau trường 5.2 Hướng phát triển đồ án tương lai Trong tương lai, để phát triển đồ án nhóm chúng em tiếp tục nghiên cứu thêm để hồn thiện mơ hình robot cải tiến mơ hình 3D giao diện điều khiển để thân thiện tiện dụng với người dùng hơn, nâng cao tính thẩm mĩ, hướng tới tích hợp cảm biến, ứng dụng xử lí ảnh để robot hoạt động độc lập không cần can thiệp người Cùng với đó, chúng em muốn đưa đề tài đến với hệ sinh viên để trở thành công cụ học tập nghiên cứu arduino,lập trình điều khiển qua Bluetooth… Cùng với nhóm tiếp tục tìm hiểu, nghiên cứu để chế tạo sản phẩn thực tế có khả leo cầu thang tòa nhà cao tầng để chuyển hàng hóa, cứu nạn… 62 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH TÀI LIỆU THAM KHẢO [1 ] [2 ] [3 ] [4 ] [5 ] [6 ] Trịnh Chất - Lê Văn Uyển, Tính tốn thiết kế dẫn động khí (tập một) Nhà xuất giáo dục, 2006 PGS TS Đào Văn Hiệp, Kỹ thuật Robot, Nhà xuất khoa học kỹ thuật, 2006 PGS TS Nguyễn Quang Hoàng, Bài giảng Robotics, 2017 Andreas Hölldorfer, BCN3D-MOVEO, github.com/BCN3D/BCN3D-Moveo GS TS Phan Bùi Khơi, Bài giảng tính tốn thiết kế robot, 2007 GS TSKH Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất khoa học kỹ thuật, 2006 63 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH PHỤ LỤC Code lập trình điều khiển #include "config.h" char c; void setup() { pinInit(); // leocauthang(); } void loop() { if (HC05.available()) { c = HC05.read(); if (c=='A') tien(); else if (c=='B') lui(); else if (c=='C') trai(); else if (c=='D') phai(); else if (c=='L') leocauthang(); else dung(); } } #include #include Servo sv1,sv2,sv3,sv4,sv5,sv6,sv7,sv8,sv9,sv10; SoftwareSerial HC05(A2, A1); int sensor=A5; void truoclen(int i) { sv4.write(115-i); sv6.write(90+i); } void saulen(int i) { sv3.write(65+i); sv5.write(90-i); } 64 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH void dung() { sv1.write(90); sv2.write(90); sv7.write(90); sv8.write(90); sv9.write(90); sv10.write(90); } void tien() { sv1.write(94); // trai sv2.write(86); // phai sv7.write(94); sv8.write(94); sv9.write(86); sv10.write(86); } void lui() { sv1.write(85); sv2.write(95); sv7.write(85); sv8.write(85); sv9.write(95); sv10.write(95); } void trai() { sv1.write(85); sv2.write(85); sv7.write(85); sv8.write(85); sv9.write(85); sv10.write(85); } 65 ĐỒ ÁN TỐT NGHIỆP BÁ GVHD: TS BÙI ĐÌNH void phai() { sv1.write(95); sv2.write(95); sv7.write(95); sv8.write(95); sv9.write(95); sv10.write(95); } void pinInit() { pinMode(sensor,INPUT); sv1.attach(2); sv2.attach(3); // banh to sv4.attach(5); sv6.attach(7); // truoc sv3.attach(4); sv5.attach(6); // sau sv7.attach(8); sv8.attach(9); // banh nho sv9.attach(10); sv10.attach(11); // banh nho dung(); saulen(0); truoclen(0); Serial.begin(9600); HC05.begin(9600); } void leocauthang() { tien(); while(digitalRead(sensor)); for (int i=0;i=-15;i ) { saulen((i-45)/2+7); truoclen(i); delay(30); } delay(100); while(digitalRead(sensor)); for (int i=-15;i