Nghiên cứu, thiết kế hệ thống điều khiển mô hình cánh tay máy 4 bậc tự do

TRƢỜNG ĐẠI HỌC LÂM NGHIỆP KHOA CƠ ĐIỆN VÀ CÔNG TRÌNH BỘ MƠN KỸ THUẬT ĐIỆN VÀ TỰ ĐỘNG HĨA o0o KHOÁ LUẬN TỐT NGHIỆP Tên đề tài: Nghiên cứu, thiết kế hệ thống điều khiển mơ hình cánh tay máy bậc tự Giáo viên hƣớng dẫn : ThS Đinh Hải Lĩnh KS Nguyễn Thành Trung Sinh viên thực : Nguyễn Phƣơng Nam Mã sinh viên : 1451050143 Lớp : 59 – CĐT Khóa : 2014- 2018 Hà Nội - 2018 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam LỜI NÓI ĐẦU Robot cơng nghiệp giữ vị trí chủ chốt nhiều ngành sản xuất tự động hóa nhƣ chế tạo tơ, chế tạo máy, điện – điện tử, chế tạo hóa chất v.v Trong nhà máy sản xuất, robot thƣờng đƣợc xếp thành hay nhiều dây chuyền có chức làm việc điều kiện khác hƣớng đến mục đích gia tăng suất lao động, có độ xác cao giảm đƣợc lƣợng nhân công định Sự tiến khoa học kỹ thuật lĩnh vực điện – điện tử, tin học năm gần đƣợc ứng dụng rộng rãi nhiều lĩnh vực khác Đặc biệt lĩnh vực điều khiển tự động, nhiều robot công nghiệp với điều khiển chúng đời ngày đại hƣớng đến cho ngƣời tiện lợi ngày rẻ Trên giới lĩnh vực đƣợc nghiên cứu, chế tạo ngày phát triển nhƣng nƣớc ta cịn hạn chế, chƣa ứng dụng đƣợc vào sản xuất cách khoa học Chính lẽ tác giả thực đề tài nghiên cứu khoa học “Nghiên cứu, thiết kế hệ thống điều khiển mơ hình cánh tay máy bậc tự do” Mặt khác, thực đề tài giúp tác giả củng cố kiến thức học robot cơng nghiệp tự động hóa có ích cho cơng việc học tập sau Tác giả xin chân thành cảm ơn thầy cô môn KTĐ&TĐH, đặc biệt thầy Nguyễn Thành Trung cô Đinh Hải Lĩnh hƣớng dẫn tận tình truyền đạt nhiều kiến thức, kinh nghiệm quý báu vi điều khiển Arduino Hà Nội, ngày 11 /05/2018 Sinh viên thực Nguyễn Phương Nam GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung TRƢỜNG ĐẠI HỌC LÂM NGHIỆP KHOA CƠ ĐIỆN VÀ CƠNG TRÌNH Bộ mơn Kỹ thuật điện & Tự động hóa SVTH Nguyễn Phƣơng Nam CỘNG HÕA XÃ HỘI CHỦ NGHĨA VIỆT NAM Độc lập – Tự – Hạnh phúc Hà Nội, ngày 14 tháng 05 năm 2018 PHIẾU NHẬN XÉT VÀ CHẤM ĐIỂM KHÓA LUẬN TỐT NGHIỆP (Dành cho giảng viên hướng dẫn) Họ tên sinh viên: Nguyễn Phƣơng Nam Mã sinh viên: 1451050143 Lớp: 59_CĐT Sinh viên Nguyễn Phƣơng Nam hồn thành báo cáo khóa luận tốt nghiệp theo quy định tiến độ kế hoạch Bộ môn Khoa đề - Về nội dung: Báo cáo khóa luận gồm 04 chƣơng: Chƣơng 1: Giới thiệu tổng quan cánh tay máy Chƣơng 2: Thiết kế phần cứng cho mơ hình cánh tay máy Chƣơng 3: Xây dựng phần mềm Chƣơng 4: Lắp đặt chạy thử nghiệm Nội dung báo cáo khóa luận hợp lý, đầy đủ bảng biểu, hình vẽ minh họa, trình bày theo mẫu quy định Đã có vận dụng, tổng hợp kiến thức môn học chuyên ngành q trình thực khóa luận Về nghiên cứu chế tạo đƣợc mơ hình cánh tay máy bậc tự sử dụng - Về ý thức: thời gian thực khóa luận tốt nghiệp, sinh viên Nguyễn Phƣơng Nam chấp hành nghiên túc lịch làm việc; chủ động việc sƣu tầm, tìm hiểu tài liệu, chế tạo đƣợc thiết bị thực nội dung khóa luận Kết luận: Đồng ý cho sinh viên Nguyễn Phƣơng Nam nộp báo cáo khóa luận bảo vệ khóa luận tốt nghiệp Giảng viên hƣớng dẫn KS Nguyễn Thành Trung GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam NHẬN XÉT (Của giảng viên phản biện) GIẢNG VIÊN PHẢN BIỆN (Chữ ký, họ tên) GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam MỤC LỤC Lời nói đầu Danh mục bảng Danh mục hình vẽ Trang CHƢƠNG 1: GIỚI THIỆU TỔNG QUAN VỀ CÁNH TAY MÁY .1 1.1 KHÁI NIỆM VỀ CÁNH TAY MÁY 1.1.1 Giới thiệu chung robot công nghiệp .1 1.1.2 Cánh tay máy bốn bậc tự 1.1.3 Một số cánh tay máy 1.2 CÁC KIỂU ĐIỀU KHIỂN CÁNH TAY ROBOT 1.3 GIỚI THIỆU VỀ BOARD MẠCH ĐIỀU KHIỂN ARDUINO 1.3.1 Khái niệm chung Arduino .3 1.3.2 Ứng dụng Arduino đời sống .4 1.3.3 Một số board Arduino thị trường 1.4 GIỚI THIỆU VỀ ĐỘNG CƠ BƢỚC SERVO VÀ CƠ CẤU CHẤP HÀNH .6 1.4.1 Khái niệm chung động servo RC MG995 1.4.2 Phân loại động servo CHƢƠNG 2: THIẾT KẾ PHẦN CỨNG CHO MƠ HÌNH CÁNH TAY MÁY 2.1 THIẾT KẾ PHẦN CƠ KHÍ CHO CÁNH TAY MÁY 2.1.1 Phương pháp denavit-hartenberg (dh) .9 2.1.2 Phân tích, lựa chọn phương án thiết kế khung khí chọn động 10 2.1.3 Thiết kế khung khí cho cánh tay máy sử dụng phần mềm Inventor 14 2.2 THIẾT KẾ SƠ ĐỒ NGUYÊN LÝ CHO MẠCH ĐIỀU KHIỂN .15 2.2.1 Lựa chọn phần mềm thiết kế 15 2.2.2 Chọn xử lý trung tâm cấu chấp hành .16 2.2.3 Các bước thiết kế sơ đồ nguyên lý cho mạch điều khiển 17 2.3 THIẾT KẾ SƠ ĐỒ MẠCH IN CHO BỘ ĐIỀU KHIỂN .21 CHƢƠNG 3: XÂY DỰNG PHẦN MỀM 24 3.1 PHƢƠNG PHÁP HỌC LỆNH CHO CÁNH TAY MÁY .24 3.2 LƢU ĐỒ THUẬT TỐN CHO CHƢƠNG TRÌNH PHẦN MỀM 25 3.3 LẬP TRÌNH PHẦN MỀM .26 3.3.1 Lựa chọn phần mềm lập trình 26 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam 3.3.2 Các bước tiến hành lập trình 27 CHƢƠNG 4: LẮP ĐẶT VÀ CHẠY THỬ NGHIỆM 35 4.1.QUY TRÌNH LẮP ĐẶT KHUNG CƠ KHÍ CHO CÁNH TAY MÁY 35 4.2 LẮP RÁP, KẾT NỐI DIỆN CHO BỘ ĐIỀU KHIỂN VÀ CHẠY THỬ NGHIÊM MƠ HÌNH CÁNH TAY MÁY 37 KẾT LUẬN VÀ KHUYẾN NGHỊ 38 PHỤ LỤC 39 TÀI LIỆU THAM KHẢO 46 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam DANH MỤC CÁC BẢNG Bảng 1: Bảng thông số DH 12 Bảng 2: So sánh thông số kỹ thuật Arduino Pro Mini Arduino UNO R3 16 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam DANH MỤC CÁC HÌNH VẼ Hình 1.1: Cánh tay máy tọa độ đề Hình 1.2: Cánh tay máy tọa độ trục Hình 3: Cánh tay máy cầu Hình 1.4: Cánh tay máy SCARA Hình 5: Cánh tay máy kiểu tay người Hình 1.6: Cánh tay máy kiểu cổ tay Hình 1.7: Arduino Uno R3 Hình 1.8: Arduino UNO R3 Hình 1.9: Arduino Nano Hình 1.10: Arduino Leonardo Hình 1.11: Arduino Pro Mini Hình 1.12: Arduino Mega Hình 1.13: Arduino Due Hình 1.14: Cấu tạo động cở servo RC MG995 Hình 1.15: Hình ảnh số loại động servo RC thường gặp: Hình 1.16: Động servo AC có gắn kèm encoder dằng sau động Hình 1.17: Động Cơ DC Giảm Tốc Servo Faulhaber 12V Hình 1: Cách xác định trục phương pháp Denavit-hartenberg Hình 2: Mơ tả số bậc tự vật không gian 11 Hình 3: Mơ hình cánh tay máy 11 Hình 4: Cấu tạo điện áp hoạt động động servo RC MG995 13 Hình 5: Ngun vật liệu để chế tạo khung khí cho cánh tay máy 14 Hình 6: Mơ hìn cánh tay máy hình chiếu 2D 14 Hình 7: Mơ hình mơ cánh tay máy 15 Hình 8: Sơ đồ chân chức chân Arduino Pro Mini 17 Hình 9: Sơ đồ cấu trúc hệ thống 17 Hình 10: Giao diện ban đầu sau cài đặt phần mềm Altium Desginer 18 Hình 11: Tạo project Altium Desginer 18 Hình 12: Đặt tên chọn địa lưu file 18 Hình 13: Add file thiết kế mạch nguyên lý vào Project 19 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam Hình 14: Add thư viện vào phần mềm Altium Desginer 19 Hình 15: Tìm lấy linh kiện ngồi mơi trường làm việc 19 Hình 16: Kết sau lấy linh kiện mơi trường làm việc 20 Hình 17: Sơ đồ nguyên lý xử lý trung tâm 20 Hình 18: Sơ đồ nguyên lý chuyển tín hiệu 21 Hình 19: Quá trình Update Schematics 21 Hình 20: Kết thu sau Update Schematics 21 Hình 21: Kết sau cài đặt thông số 22 Hình 22: Sơ đồ mạch in xử lý trung tâm (mơi trường 2D) 22 Hình 23: Sơ đồ mạch in xử lý trung tâm (mơi trường 3D) 23 Hình 24: Sơ đồ mạch in chuyển tín hiệu (mơi trường 2D) 23 Hình 25: Sơ đồ mạch in chuyển tín hiệu (mơi trường 2D) 23 Hình 1: Lưu đồ thuật toán 25 Hình 2: Giao diện Arduino IDE 26 Hình 3: Mơ tả dấu hiệu nhận biết thư viện 29 Hình 4: Cửa sổ add thư viện online 30 Hình 5: Cửa sổ add thư viện phương pháp thủ công 30 Hình 6: Kết nối arduino với máy tính 31 Hình 7: Tìm kiếm Device Manager cửa sổ Run 31 Hình 8: Giao diện Device Manager 32 Hình 9: Kiểm tra cổng kết nối Arduino 32 Hình 10: Vị trí phần mềm Arduino 32 Hình 11: Chọn loại mạch Aruino phần mềm 33 Hình 12: Chọn cổng làm việc cho Arduino 33 Hình 13: Cách xác nhận cổng làm việc phần mềm 33 Hình 14: Chọn cách nạp Chip 34 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam Hình 1: bàn xoay cánh tay máy 35 Hình 2: Cớ cấu khớp cánh tay máy 35 Hình 3: Cơ cấu khớp hai cánh tay máy 35 Hình 4: Khớp hai khớp ba cánh tay máy 36 Hình 5: Kế nối tay gắp với khớp ba vủa cánh tay mày 36 Hình 6: Khung khí hoản chỉnh cánh tay máy 36 Hình 7: Khối điều khiển cánh tay máy 37 Hình 8: Kết nối chạy thử nghiệm mơ hình cánh tay máy 37 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam Hình 8: Giao diện Device Manager Mở mục Ports (COM & LPT), ta thấy cổng COM arduino pro mini kết nối Hình 9: Kiểm tra cổng kết nối Arduino Cổng kết nối COM14 Thông thƣờng, lần kết nối tiếp theo, Windows sử dụng lại cổng COM14 để kết nối nên ta không cần thực thêm thao tác tìm cổng COM Bước 3: Khởi động Arduino IDE Hình 10: Vị trí phần mềm Arduino Bước 4: Cấu hình cho phần mềm Arduino IDE Vào menu Tools Board chọn Arduino Pro or Pro Mini 32 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam Hình 11: Chọn loại mạch Aruino phần mềm Vào menu Tools Serial Port chọn cổng Arduino kết nối với máy tính Hình 12: Chọn cổng làm việc cho Arduino Xác nhận cổng COM Arduino IDE góc dƣới bên phải cửa sổ làm việc Hình 13: Cách xác nhận cổng làm việc phần mềm 33 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung Vào menu Tools SVTH Nguyễn Phƣơng Nam Programmer chọn AVR ISP Hình 14: Chọn cách nạp Chip Bước 5: Mở nạp mã nguồn chƣơng trình: Mở chƣơng trình điều khiển (Phụ lục) sau bấm tổ hợp phím Ctrl + U để tải chƣơng trình lên mạch Arduino Pro Mini Arduino IDE xác nhận nạp thành cơng có dịng chữ “Done uploading” Một số lỗi nhƣ khơng nạp đƣợc chƣơng trình ta nên kiểm tra cổng COM máy tính xem cài đặt chƣa khơng chọn tên cổng Ngồi cịn chip Atmega328 bị treo ta nên nạp thông qua chuẩn SPI 34 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam CHƢƠNG LẮP ĐẶT VÀ CHẠY THỬ NGHIỆM 4.1 Quy trình lắp đặt khung khí cho cánh tay máy Để lắp ráp khung khí cho cánh tay máy cách khoa học dễ dàng thực quy trình lắp ráp theo bƣớc sau Bước 1: Đầu tiên cần phải lắp ráp đế với bàn xoay cho cánh tay máy Hình 1: Bàn xoay cánh tay máy Bước 2: Sau lắp ráp song phần đế với bàn xoay với ta tiếp tục lắp ráp tới cấu khớp cánh tay máy Hình 2: Cớ cấu khớp cánh tay máy Bước 3: Lắp ráp cấu khớp hai cánh tay máy Hình 3: Cơ cấu khớp hai cánh tay máy Bước 4: Kết nối khớp hai với khớp ba cánh tay máy 35 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam Hình 4: Khớp hai khớp ba cánh tay máy Bước 5: Tiến hành kế nối tay gắp với khớp ba vủa cánh tay mày Hình 5: Kế nối tay gắp với khớp ba vủa cánh tay mày Bước 6: Thực lắp ráp khớp với khớp hai cánh tay máy thực lắp ráp khớp lên bàn xoay cánh tay máy Hình 6: Khung khí hoản chỉnh cánh tay máy 36 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung 4.2 SVTH Nguyễn Phƣơng Nam Lắp ráp, kết nối diện cho điều khiển chạy thử nghiêm mơ hình cánh tay máy Để dễ dàng thực thao tác trình hàn mạch cần phải thực hàn linh cà linh kiện từ thấp đến cao Hồn thành cơng đoạn hàn mạch bắt đầu kết nối khơi khối lại với đóng hộp Ta đƣợc kế nhƣ hình 4.7 Hình 7: Khối điều khiển cánh tay máy Hình 8: Kết nối chạy thử nghiệm mơ hình cánh tay máy Sau kết nối chạy thử nghiệm mô hình cánh tay máy thu đƣợc kết nhƣ mong muốn.Chúng ta điều khiển khớp quay cánh tay máy biến trở Cánh tay máy thực nhớ thao tác trƣớc mà điều khiển nó, cánh tay máy gắp di chuyển vật từ vị trí sang vị trí khác 37 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam KẾT LUẬN VÀ KHUYẾN NGHỊ Trải qua thời gian tìm hiểu nghiên cứu, xây dựng ý tƣởng bắt tay vào thi cơng Tác giả hồn thành đề tài “Nghiên cứu, thiết kế hệ thống điều khiển mơ hình cánh tay máy bậc tự do” Mơ hình đƣợc thiết kế chế tạo vật liệu mica, vận hành tảng điều khiển động servo RC cách sử dụng Arduino xử lý phân tích tín hiệu Analog biến trở tạo Chế tạo thành cơng mơ hình cánh tay máy bốn bậc tự chạy thử nghiệm phịng thí nghiệm Kỹ thuật điện Tự động hóa khoa Cơ điện Cơng trình Kết hoạt động cánh tay máy theo tín hiệu điều khiển Tuy nhiên hạn chế mặt thời gian nên mơ hình cánh tay máy cịn có số lỗi nhƣ khớp quay chƣa đƣợc ổn định Trên sở mô hình này, hƣớng phát triển mơ hình xây dựng hệ thống điều khiển phƣơng pháp điều khiển không dây cánh tay máy hoạt động cánh xác, kịp thời theo tín hiệu điều khiển 38 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam PHỤ LỤC I Chƣơng trình điều khiển mơ hình cánh tay máy #include #include #define servo_max // số step lớn , tiêu tốn step_max*servo_max (byte) RAM #define step_max 200 #define start_pause_pin #define record_pin #define ENABLE_EEPROM_PIN #define led_pin 12 #define delay_toc_do 20 byte pin_analog[servo_max] = { A0, A1, A2, A3}; byte pin_servo[servo_max] = { 3, 5, 6, 9}; unsigned int A0_value, A1_value, A2_value, A3_value byte get_goc(byte servo_i) { switch (servo_i) { case 0: A0_value = constrain(analogRead(A0), 200, 823); return map(A0_value, 200, 823, 0, 180); break; case 1: A1_value = constrain(analogRead(A1), 200, 823); return map(A1_value, 200, 823, 4, 175); break; case 2: A2_value = constrain(analogRead(A2), 200, 823); return map(A2_value, 200, 823, 175, 4); break; case 3: A3_value = constrain(analogRead(A3), 200, 823); return map(A3_value, 200, 823, 175, 4); break; default: 39 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam break; } } const unsigned int SIZE_MEMORY_EEPROM = 1024; Servo servo[servo_max]; int step_move = 0; byte goc_servo[servo_max][step_max]; byte goc_tam_thoi[servo_max] = { 90 }; void setup() { pinMode(led_pin, OUTPUT); pinMode(start_pause_pin, INPUT_PULLUP); pinMode(ENABLE_EEPROM_PIN, INPUT_PULLUP); pinMode(record_pin, INPUT_PULLUP); for (byte i = 0; i < servo_max; i++) { pinMode(pin_analog[i], INPUT); servo[i].attach(pin_servo[i]); } ADCSRA = ((ADCSRA & (B11111000)) | B00000100); learn_and_move(); } void move_servo(byte i, byte goc_i) { servo[i].write(goc_i); } void nhap_nhay(unsigned int time_delay, byte count) { for (byte i = 0; i < count; i++) { digitalWrite(led_pin, HIGH); delay(time_delay); digitalWrite(led_pin, LOW); delay(time_delay * 2); } } void record(unsigned int step_x) { 40 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam for (byte i = 0; i < servo_max; i++) { goc_servo[i][step_x] = get_goc(i); } nhap_nhay(100, 2); Serial.println(goc_servo[0][step_x]); } void move_all() { for (byte i = 0; i < servo_max; i++) { move_servo(i, goc_tam_thoi[i]); } } void control_servo() { byte hieu; for (byte i = 0; i < servo_max; i++) { hieu = abs(goc_tam_thoi[i] - get_goc(i)); if ((hieu >= 1) && (hieu < 170)) { goc_tam_thoi[i] = get_goc(i); } } move_all(); } void nap_eeprom_sang_ram() { step_move = EEPROM.read(SIZE_MEMORY_EEPROM - 1); for (byte i = 0; i < servo_max; i++) { for (int step_j = 0; step_j < step_max; step_j++) { goc_servo[i][step_j] = EEPROM.read(step_j + i * step_max); } } } void luu_vao_eeprom() { 41 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam EEPROM.write(SIZE_MEMORY_EEPROM - 1, step_move); delay(15); nhap_nhay(500, 3); digitalWrite(led_pin, HIGH); for (byte i = 0; i < servo_max; i++) { for (int step_j = 0; step_j < step_move; step_j++) { EEPROM.write(step_j + i * step_max, goc_servo[i][step_j]); delay(15); } } nhap_nhay(500, 3); digitalWrite(led_pin, LOW); } void pause() { if (digitalRead(start_pause_pin) == 0) { while (true) { digitalWrite(led_pin, 1); delay(300); Serial.println("PAUSE"); if (digitalRead(ENABLE_EEPROM_PIN) == 0) { delay(300); luu_vao_eeprom(); } if (digitalRead(start_pause_pin) == 0) { Serial.println("START"); digitalWrite(led_pin, 0); delay(300); goto out_pause; } } } out_pause; } void auto_move() 42 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam {Serial.println(step_move); nhap_nhay(50, 3); float hieu_f[servo_max]; unsigned int step = 0, step_next; byte time; byte thay_doi; while (true) { if (step < step_move) { step_next = step + 1; } else { step_next = 0; } for (byte i = 0; i < servo_max; i++) { hieu_f[i] = (float(goc_servo[i][step]) - float(goc_servo[i][step_next])); } Serial.println(step); Serial.println(goc_servo[0][step]); Serial.println(goc_servo[0][step_next]); Serial.println(hieu_f[0]); for (float loading = 1.0; loading = 0.0) && (loading < 20.0)) { time = delay_toc_do * + 5; } 43 GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam else if (loading < 30.0) { time = delay_toc_do / + 5; } else if (loading < 90.0) { time = delay_toc_do + 5; } else { time = delay_toc_do * + 5; } while ((millis() % delay_toc_do) != 0) { // làm trễ pause(); } //while move_all(); } if (step < step_move) { step++; } else { step = 0; } delay(100); } } void learn_and_move() { step_move = 0; while (digitalRead(start_pause_pin) != 0) { while (digitalRead(record_pin) != 0) { control_servo(); if ((digitalRead(start_pause_pin) (step_move == 0)) { eeprom nap_eeprom_sang_ram(); } 44 == 0) && GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung if SVTH Nguyễn Phƣơng Nam ((digitalRead(start_pause_pin) (step_move != 0)) { goto buoc_3; } } record(step_move); if (step_move < step_max) { step_move++; } } buoc_3: step_move ; auto_move(); } void loop() { } 45 == 0) && GVHG Đinh Hải Lĩnh GVHG Nguyễn Thành Trung SVTH Nguyễn Phƣơng Nam TÀI LIỆU THAM KHẢO Điều khiển robot công nghiệp(2007), Nguyễn Mạnh Tiến, Trƣờng ĐH Bách Khoa Hà Nội Lập trình điều khiển với Arduino, Phạm Quang Huy – Lê Cảnh Trung, Nhà xuất Kho học Kỹ thuật Vy điều khiển ứng dụng arduino dành cho người tự học, Phạm Quang Huy – Nguyễn Trọng Hiếu, Nhà xuất Bách Khoa Hà Nội http://arduino.vn/ https://circuitdigest.com/tech-articles https://github.com/ https://www.arduino.cc/ http://www.kythuatchetao.com/canh-tay-robot-bai-1-cac-khai-niem-co-ban-vaphan-loai-robot/ 46