Thiết kế robot phun thuốc trừ sâu

ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ ĐỒ ÁN TỐT NGHIỆP ĐỀ TÀI: THIẾT KẾ ROBOT PHUN THUỐC TRỪ SÂU Người hướng dẫn: Sinh viên thực hiện: TS ĐẶNG PHƯỚC VINH VŨ ĐÌNH TUÂN TRẦN PHẠM QUANG VIỆT Số thẻ sinh viên : 101120324 101120380 Lớp: 12CDT2 Đà Nẵng, 2017 TÓM TẮT ĐỒ ÁN TỐT NGHIỆP Tên đề tài : THIẾT KẾ ROBOT PHUN THUỐC TRỪ SÂU Họ tên SV : Vũ Đình Tuân Mã SV : 101120324 Lớp : 12CDT1 Họ tên SV : Trần Phạm Quang Việt Mã SV :101120380 Lớp : 12CDT2 GV hướng dẫn : T.S Đặng Phước Vinh GV duyệt : PGS.TS Trần Xuân Tùy Nội dung làm bao gồm vấn đề sau: Nhu cầu thực tế đề tài Đồ án Tốt nghiệp học phần bắt buộc sinh viên ngành Cơ Điện Tử nói riêng sinh viên khối ngành kĩ thuật nói chung Để thiết kế thành công cấu, cụm chi tiết máy hay máy hồn chỉnh địi hỏi sinh viên phải có hiểu biết nắm kiến thức lĩnh vực khí điện tử Đây điều kiện thuận lợi cho chúng em để ứng dụng kiến thức học vào thực tế Nền nơng nghiệp nước ta cịn lạc hậu chưa có nhiều ứng dụng khoa học kĩ thuật áp dụng vào thực tế Rất nhiều quy trình kĩ thuật trồng trọt, chăm sóc người nơng dân tiếp xúc trực tiếp với loại thuốc trừ sâu độc hại mà khơng có dụng cụ bảo vệ, bên cạnh suất đạt lại thấp, khả thực diện rộng không cao Vấn đề đặt sức khỏe cho người nông dân đặt lên hàng đầu, tiếp theolà suất hiệu kinh tế cao Chính thiết bị phun thuốc trừ sâu tự động nghiên cứu, thiết kế, đưa vào thực tiễn ngày áp dụng nhiều Và chúng đề tài mà chúng em muốn hướng tới “ROBOT Phun Thuốc Trừ Sâu” Vì nước ta canh tác nơng nghiệp chưa thật đại chưa có hệ thống định nên vườn khác người ta trồng với chiều rộng luống khác tùy loại nên Robot thiết kế di chuyển tất luống dựa vào khả đóng mở cụm bánh xe, Robot điều khiển từ xa nhờ thiết bị mạch điều khiển để đảm bảo an toàn tuyệt đối cho người nơng dân Vịi phun thiết kế nằm ngang tăng giảm khoảng cách phun từ vòi phun so với mặt đất cách tự động Ngồi Robot gắn hệ thống cày đất giúp Robot có khả làm việc đa hiệu Phạm vi nghiên cứu đề tài tốt nghiệp: Trong đề tài chúng em thiết kế cấu cho máy, tìm hiều phương pháp điều khiển để đưa phương án tối ưu nhất, nghiên cứu tính thiết thực sản đưa giải pháp phù hợp để đảm bảo suất chất lượng sản phẩm, từ đó: - Tính tốn thiết kế hệ truyền động - Thiết kế thi cơng mơ hình Nội dung đề tài thực : - Số trang thuyết minh: 75 - Số vẽ: 5A0 - Mơ hình: mơ hình robot phun thuốc trừ sâu Kết đạt được: * Phần lý thuyết tìm hiểu: - Tổng quan robot phun thuốc trừu sâu, tính thiết thực cấp thiết đề tài - Các sản phẩm robot trừu sâu giới - Giới thiệu số truyền, lựa chọn phương án thiết kế cấu dẫn động cho robot phun thuốc trừ sâu - Tính toán lựa chọn thành phần, truyền sử dụng robot phun thuốc trừ sâu - Thiết kế phận gá đặt robot - Giới thiệu thành phần module chức điều khiển động * Đã lựa chọn thiết kế phần: - Lựạ chọn đưa phương án tối ưu - Lựa chọn cấu truyền động cho robot - Chọn loại động sử dụng - Bộ truyền bánh răng, bánh vít trục vít, cấu nâng hạ cần phun theo cấu hình bình hành - Nghiên cứu mạch điều khiển để điều khiển cho robot hoạt động từ xa * Kèm mơ hình robot phun thuốc trừ sâu ĐẠI HỌC ĐÀ NẴNG TRƯỜNG ĐẠI HỌC BÁCH KHOA KHOA CƠ KHÍ CỘNG HỊA XÃ HÔI CHỦ NGHĨA VIỆT NAM Độc lập - Tự - Hạnh phúc NHIỆM VỤ ĐỒ ÁN TỐT NGHIỆP a TT Họ tên sinh viên Số thẻ SV Vũ Đình Tuân 101120324 12CDT1 CƠ ĐIỆN TỬ Trần Phạm Quang Việt 101120380 12CDT2 CƠ ĐIỆN TỬ Lớp Ngành Tên đề tài: THIẾT KẾ ROBOT PHUN THUỐC TRỪ SÂU Đề tài thuộc diện: ☐ Có ký kết thỏa thuận sở hữu trí tuệ kết thực Các số liệu liệu ban đầu: Nội dung phần thuyết minh tính toán: Phần chung: TT Họ tên sinh viên Vũ Đình Tuân Trần Phạm Quang Việt Nội dung - Tìm hiểu số loại robot phun thuốc trừ sâu thực tế đưa ý tưởng để thiết kế - Đưa nguyên lí, lựa chọn cấu phù hợp để thiết kế - Thi công robot thực tế b Phần riêng TT Họ tên sinh viên Nội dung Vũ Đình Tuân Thiết kế robot solidworks, tìm hiểu code điều khiển Trần Phạm Quang Việt Gia cơng số chi tiết, hồn thành thuyết minh Các vẽ, đồ thị ( ghi rõ loại kích thước vẽ ): a Phần chung: TT Họ tên sinh viên Vũ Đình Tuân Trần Phạm Quang Việt Nội dung - Bản vẽ tổng thể 1A0 b Phần riêng: TT Họ tên sinh viên Vũ Đình Tuân Trần Phạm Quang Việt Nội dung - Bản vẽ thuật toán 1A0 - Bản vẽ sơ đồ động 1A0 - Bản vẽ sơ đồ chi tiết 1A0 - Bản vẽ sơ đồ mạch điện 1A0 Họ tên người hướng dẫn: T.S Đặng Phước Vinh Ngày giao nhiệm vụ đồ án: 16/2/2017 Ngày hoàn thành đồ án: 20/5/ 2017 Đà Nẵng, ngày Trưởng Bộ môn Kỹ thuật Cơ điện tử tháng Người hướng dẫn năm2017 LỜI NÓI ĐẦU Chúng em xin chân thành cảm ơn thầy cô khoa Cơ khí mơn Cơ điện tử trường Đại Học Bách Khoa, Đại Học Đà Nẵng tận tụy dạy dỗ, truyền đạt cho chúng em kiến thức quý báu năm học vừa qua để chúng em có kiến thức hồn thành đề tài Chúng em xin chân thành cảm ơn thầy Đặng Phước Vinh tận tình hướng dẫn, bảo chúng em suốt thời gian học tập thực đề tài Xin chân thành cảm ơn bạn tập thể lớp 12CDT tham gia đóng góp ý kiến suốt q trình thực để nhóm hoàn thành tốt đề tài Mặc dù hướng dẫn nhiệt tình thầy Đặng Phước Vinh, cơng nghệ cịn hạn chế, kinh nghiệm thiết kế chưa trau dồi nhiều, tài liệu phục vụ cho công việc thiết kế cịn q nên khơng tránh khỏi bỡ ngỡ sai sót Sau thời gian tháng làm đề tài nổ lực thân hướng dẫn thầy Đặng Phước Vinh, thầy cô giáo giúp đỡ bạn sinh viên khác khoa chúng em hoàn thành xong đồ án thời gian quy định Một lần cho phép chúng em gửi đến q thầy bạn lịng biết ơn sâu Đà Nẵng, ngày tháng năm 2017 Nhóm sinh viên thực hiện: Vũ Đình Tuân i Trần Phạm Quang Việt CAM ĐOAN Kính gửi: - Trường Đại Học Bách Khoa Đà Nẵng - Khoa Cơ Khí Chúng em xin cam đoan nội dung đồ án chép đồ án hay cơng trình có trước Mọi giúp đỡ cho việc thực đồ án cảm ơn, thơng tin trích dẫn đồ án ghi nguồn gốc rõ ràng phép cơng bố Nhóm sinh viên thực hiện: Vũ Đình Tuân ii Trần Phạm Quang Việt MỤC LỤC LỜI NÓI ĐẦU CHƯƠNG I: TỔNG QUAN ĐỀ TÀI 1.1 Đặt vấn đề tính cấp thiết đề tài 1.2 Mục đích nghiên cứu 1.3 Ý tưởng thiết kế robot “ Phun thuốc trừ sâu” CHƯƠNG 2: TÍNH TỐN THIẾT KẾ CƠ KHÍ CHO ROBOT 2.1 Lựa chọn phương án tính tốn thiết kế robot 2.1.1 Lựa chọn phương án thiết kế cụm mở rộng khoảng cách bánh xe 2.1.1.1 Phương án thứ “Thiết kế động truyền mô men cho thơng qua truyền ngồi“ 2.1.1.2 Phương án thứ hai “Thiết kế động truyền mô men cho hai cần quay thông qua truyền “ 2.1.2 Tính tốn truyền chọn động 14 2.1.2.1 Chọn động 15 2.1.2.2 Tính tốn truyền 21 2.2 Lựa chọn phương án tính tốn cụm bánh xe 24 2.2.1 Lựa chọn phương án 24 2.2.2 Tính tốn cụm bánh xe 27 2.2.3 Thiết kế tính tốn chọn động cụm lái bánh xe 31 2.2.3.1 Thiết kế cụm lái bánh xe 31 2.2.3.2 Tính tốn chọn động lái bánh xe 33 2.2.4 Thiết kế cụm cần phun 39 2.2.4.1 Lựa chọn phương án thiết kế 39 2.2.4.2 Tính chọn động 41 2.2.5 Lựa chọn cách thức phun 44 2.2.5.1 Cấu tạo máy phun thuốc nước 44 2.2.5.2 Chọn động bơm 45 CHƯƠNG 3: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN VÀ HỆ THỐNG ĐIỆN CHO ROBOT 47 iii 3.1 Sơ đồ hệ thống điện 47 3.1.1 Sơ đồ mạch điện điều khiển 47 3.1.2 Sơ đồ kết nối hệ thống thu phát tín hiệu video 48 3.1.3 Giới thiệu sơ lược ARDUINO MEGA 2560 48 Thông số kĩ thuật 49 3.1.4 Một số phần từ khác mạch điện 52 3.1.4.1 Modun điều khiển động VNH2SP30 52 3.1.5 Mạch điều khiển động bơm L298 53 3.1.5.1 Chức chân 53 3.1.5.2 Sơ đồ nguyên lý: 53 3.1.6 Bộ thu tín hiệu RadioLink AT9-R9DS 2.4GHz Channel DSSS Receiver 54 3.1.7 Tay cầm điều khiển từ xa RadioLink AT9-R9DS 2.4GHz 55 3.1.8 Camera HD 700 TVL Mini Video For FPV ZMR250MM 210MM NTSC CAM Angle Adjustable 56 3.1.9 Bộ nhận tín hiệu không dây gửi từ camera FPV 5.8G200mW TS351+RC805 57 3.1.10 Màn hình hiển thị LCD inch 720×576 (PAL) 720×480 (NTSC) 5.8Ghz 32CH FPV Monitor & Receiver With DVR SkyZone RC800 DVR 58 CHƯƠNG 4: ĐIỀU KHIỂN VÀ LẬP TRÌNH 60 4.1 Giới thiệu thuật toán PID điều khiển vị trí động 60 4.2 Điều khiển động DC 67 4.3 Giới thiệu giao tiếp I2C arduino với 68 4.4 Chương trình 70 Kết luận: 76 Tài liệu tham khảo 77 iv Thiết kế robot phun thuốc trừ sâu việc dễ dàng phụ thuộc vào nhiều yếu tố Tơi tóm tắt kinh nghiệm chọn hệ số cho PID sau: - Chọn Kp trước: thử điều khiển P với đối tượng thật (hoặc mô phỏng), điều chỉnh Kp cho thời gian đáp ứng đủ nhanh, chấp nhận overshot nhỏ - Thêm thành phần D để loại overshot, tăng Kd từ từ, thử nghiệm chọn giá trị thích hợp Steady state error xuất - Thêm thành phần I để giảm steady state error Nên tăng Ki từ bé đến lớn để giảm steady state error đồng thời không overshot xuất trở lại Cũng có phương pháp phổ biến dùng để chọn hệ số cho điều khiển PID gọi Ziegler–Nichols ❖ Điều khiển PID số - Công thức điều khiển PID trình bày (3) dạng hàm liên tục biến e, có thành phần tuyến tính, đạo hàm tích phân Tuy nhiên, hệ thống máy tính vi điều khiển lại hệ thống số Muốn xây dựng điều khiển PID máy tính hay vi điều khiển phải biết cách xấp xỉ phương trình liên tục thành dạng rời rạc Để thực “số hóa” điều khiển PID trước hết tơi nói sơ qua hệ thống số (digital) so với hệ thống liên tục hay hệ thống tương tự (analog) Hãy quan sát hệ thống điều chỉnh nhiệt độ đơn giản hình sau Hình 4.2: Mơ hình tự động điều chỉnh nhiệt độ Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 63 Thiết kế robot phun thuốc trừ sâu - Giả sử cần điều chỉnh nhiệt độ phịng mức (tùy theo giá trị tham chiếu) quạt Cảm biến đo nhiệt độ hồi tiếp khuyếch đại vi sai (so sánh khuyếch đại) Nếu có sai số giá trị tham chiếu giá trị đo từ cảm biếm, khuyếch đại vi sai tự động khuyếch đại sai số làm tăng hay giảm vận tốc quạt để điều chỉnh nhiệt độ Quá trình xảy cách liên tục Bộ khuyếch đại vi sai trường hợp điều khiển tương tự (analog controller) Bộ khuyếch đại mạch điện tử thông thường Opamp chẳng hạn Nếu thay khuyếch đại vi điều khiển Arduino q trình hiệu chỉnh khơng cịn xảy liên tục mà theo chu kỳ Ví dụ 10 ms đọc giá trị từ cảm biến lần để tính toán sai số xuất giá trị điều khiển quạt Bộ điều khiển Arduino thực gọi điều khiển số (digital controller) khoảng thời gian 10ms gọi thời gian lấy mẫu (sampling time), khoảng cách lần điều khiển liên tiếp Rõ ràng thời gian lấy mẫu nhỏ (tấn số cao) việc hiệu chỉnh tiến gần đến “liên tục” chất lượng điều khiển tốt Trong điều khiển số, thời gian lấy mẫu yếu tố quan trọng Cần tính tốn để thời gian không lớn đừng q nhỏ, hao phí thời gian thực thi - Vì điều khiển PID xây dựng Arduino điều khiển số, cần xấp xỉ công thức điều khiển theo khoảng thời gian rời rạc Trước hết, thành phần P tương đối đơn giản quan hệ tuyến tính Kp*e, cần áp dụng trực tiếp công thức mà không cần xấp xỉ Tiếp đến xấp xỉ cho đạo hàm biến e Vì thời gian lấy mẫu cho điều khiển thường bé nên xấp xỉ đạo hàm thay đổi e lần lấy mẫu liên tiếp: de (e(k ) − e(k − 1)) = dt h Trong e  (k) giá trị e, e  (k-1) giá trị e lần lấy mẫutrước h khoảng thời gian lấy mẫu (h số) Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 64 Thiết kế robot phun thuốc trừ sâu Hình 4.3: Xấp xỉ đạo hàm biến sai số e - Thành phần tích phân xấp xỉ diện tích vùng giới hạn hàm đường biểu diễn e trục thời gian Do việc tính tốn tích phân khơng cần q xác, dùng phương pháp xấp xỉ đơn giản xấp xỉ hình chữ nhật (sai số phương pháp lớn nhất) Ý tưởng trình bày hình: Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 65 Thiết kế robot phun thuốc trừ sâu Hình 4.4: Xấp xỉ tích phân biến sai số e - Tích phân biến e tính tổng diện tích hình chữ nhật thời điểm xét Mỗi hình chữ nhật có chiều rộng thời gian lấy mẫu h chiều cao giá trị sai số e thời điểm xét Tổng quát: t k 0  edt =  e  k  h (4) Tổng hợp xấp xỉ, công thức điều khiển PID số trình bày (5) k (e(k ) − e(k − 1)) u = Kp  e + +  edt =  e  k  h h 0 t (5) Trong u đại lượng output từ điều khiển Để đơn giản hóa việc tính thành phần tích phân, nên dùng phương pháp “cộng dồn” (hay đệ quy): I (k ) = I (k −1) + e(k )  h (6) Với I(k) thành phần tích phân I(k-1) thành phần tích phân trước Các công thức (5) (6) dễ dàng để thực Arduino Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 66 Thiết kế robot phun thuốc trừ sâu 4.2 Điều khiển động DC - Điều khiển động DC (DC Motor) ứng dụng thuộc dạng điều khiển tự động DC Motor cấu chấp hành (actuator) dùng nhiều hệ thống tự động (ví dụ robot) Có phương pháp điều khiển động DC analog digital Mục đích dùng Arduio điều khiển động DC nên phương pháp số mà cụ thể phương pháp điều rộng xung (PWM) giới thiệu Ngồi ra, nói đến điều khiển động DC có đại lương điều khiển vị trí (số vịng quay) vận tốc Điều khiển động dùng PID điều khiển cách tự động nên cần đọc đại lượng điều khiển (cụ thể vị trí) hồi tiếp (feedback) để “hiệu chỉnh” PWM cấp cho động Ta dùng incremental optical encoder để đọc số vòng quay hồi tiếp cho Arduino Bộ điều khiển PID dùng vận hành Arduino Tổng quát: - Arduino phát PWM, với giá trị PWM tính từ hàm PID để tính sai số từ giá trị đặt giá trị đo từ điều khiển động thông qua điều khiển driver để vị trí trục động bám theo vị trí đặt mà ta mong muốn - Xung PWM không trực tiếp làm quay động mà thông qua mạch công suất gọi dirver ❖ Điều khiển động encoder - Để điều khiển số vòng quay hay vận tốc động thiết phải đọc góc quay motor Một số phương pháp dùng để xác định góc quay motor bao gồm tachometer (thật tachometer đo vận tốc quay), dùng biến trở xoay, dùng encoder Trong phương pháp phương pháp analog dùng optiacal encoder (encoder quang) thuộc nhóm phương pháp digital Hệ thống optical encoder bao gồm nguồn phát quang (thường hồng ngoại – infrared), cảm biến quang đĩa có chia rãnh Optical encoder lại chia thành loại: encoder tuyệt đối (absolute optical encoder) encoder tương đối (incremental optical encoder) Trong đa số Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 67 Thiết kế robot phun thuốc trừ sâu DC Motor, incremental optical encoder dùng mơ hình động servo không ngoại lệ Encoder tức incremental encoder Hình 4.5 : Bộ mã hóa quang học 4.3 Giới thiệu giao tiếp I2C arduino với ❖ Lịch sử phát triển chuẩn giao tiếp I2C - Đầu năm 1980 Phillips phát triển chuẩn giao tiếp nối tiếp dây gọi I2C I2C tên viết tắt cụm từ Inter-Intergrated Circuit Đây đường Bus giao tiếp IC với I2C phát triển bới Philips, nhiều nhà sản xuất IC giới sử dụng I2C trở thành chuẩn cơng nghiệp cho giao tiếp điều khiển, kể vài tên tuổi Philips như: Texas Intrument(TI), MaximDallas, analog Device, National Semiconductor Bus I2C sử dụng làm bus giao tiếp ngoại vi cho nhiều loại IC khác loại Vi điều khiển 8051, PIC, AVR, ARM chip nhớ như: RAM tĩnh (Static Ram), EEPROM, chuyển đổi tương tự số (ADC), số tương tự(DAC), IC điểu khiển LCD, LED - ❖ Cấu tạo vả nguyên lý hoạt động I2C sử dụng hai đường truyền tín hiệu: Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 68 Thiết kế robot phun thuốc trừ sâu • Một đường xung nhịp đồng hồ(SCL) Master phát ( thông thường 100kHz 400kHz Mức cao 1Mhz 3.4MHz) • - Một đường liệu(SDA) theo hướng Có nhiều thiết bị kết nối vào bus I2C, nhiên không xảy chuyện nhầm lẫn thiết bị, thiết bị nhận bởỉ địa với quan hệ chủ/tớ tồn suốt thời gian kết nối Mỗi thiết bị hoạt động thiết bị nhận truyền liệu hay vừa truyền vừa nhận Hoạt động truyền hay nhận cịn tùy thuộc vào việc thiết bị chủ (master) tớ (slave) - Một thiết bị hay IC kết nối với bus I2C, địa (duy nhất) để phân biệt, cịn cấu hình thiết bị chủ hay tớ.Tại lại có phân biệt ? Đó bus I2C quyền điều khiển thuộc thiết bị chủ Thiết bị chủ nắm vai trò tạo xung đồng hồ cho toàn hệ thống, hai thiết bị chủtớ giao tiếp thiết bị chủ có nhiệm vụ tạo xung đồng hồ quản lý địa thiết bị tớ suốt trình giao tiếp Thiết bị chủ giữ vai trò chủ động, thiết bị tớ giữ vai trò bị động việc giao tiếp - Về lý thuyết lẫn thực tế I2C sử dụng bit để định địa chỉ, bus có tới 2^7 địa tương ứng với 128 thiết bị kết nối, có 112 , 16 địa cịn lại sử dụng vào mục đích riêng Bit cịn lại quy định việc đọc hay ghi liệu (1 write, read) - Điểm mạnh I2C hiệu suất đơn giản nó: khối điều khiển trung tâm điều khiển mạng thiết bị mà cần hai lối điều khiển - Ngồi I2C cịn có chế độ 10bit địa tương đương với 1024 địa chỉ, tương tự bit, có 1008 thiết bị kết nối, lại 16 địa dùng để sử dụng mục đích riêng (Mình chưa rõ lắm) Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 69 Thiết kế robot phun thuốc trừ sâu Hình 4.6 : Sơ đồ hệ giao tiếp chuẩn I2C - Mỗi thiết bị có địa cài sẵn địa thiết bị để thiết bị chủ (Master) giao tiếp chân SDA VÀ SCL chân giao tiếp I2C, chân SCL chân Clock, có tác dụng đồng hóa việc truyền liệu thiết bị, việc tạo xung clock thiết bị chủ (Master) Chân lại chân SDA chân truyền liệu (DATA) chân hoạt động chế độ mở, để sử dụng cần phải có trở kéo tức nối +5v => trở => I2C thiết bị bus I2C hoạt động mức thấp Giá trị thường sử dụng cho điện trở từ 2K cho tốc độ vào khoảng 400 kbps, 10K cho tốc độ thấp khoảng 100 kbps 4.4 Chương trình ❖ Chương trình master viết cho Arduino Mega 2560 #include //#include #define INPUT_PIN 10 int analog=0; void setup() { pinMode(INPUT_PIN, INPUT); Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 70 Thiết kế robot phun thuốc trừ sâu Serial.begin(9600); Wire.begin(); // join i2c bus (address optional for master) } void loop() { int inSignal = getSignal(); analog= map(inSignal,1000,2000,0,255); Wire.beginTransmission(6); // transmit to device #6 Wire.write( analog); // sends one byte Wire.endTransmission(); // stop transmitting Wire.beginTransmission(7); // transmit to device #6 Wire.write( analog); // sends one byte Wire.endTransmission(); // stop transmitting Serial.println(inSignal); } long oldSignal = 1500; unsigned long getSignal() { unsigned long signal = pulseIn(INPUT_PIN, HIGH, 20000); if (signal < 1000 || signal > 2000) signal = oldSignal; else oldSignal = signal; return signal; } ❖ Chương trình điều khiển PID vị trí động viết cho slaver arduino Nano #include #include #define INA1 //chân INA1 driver VNH2SP( D7) #define INA2 //chân INA2 driver VNH2SP( D4) #define pin_pwm //chân PWM driver VNH2SP( D5) Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 71 Thiết kế robot phun thuốc trừ sâu #define ch1 12 // kenh encoder ////////// int xung1 =0; int xung_setup1 =0; int analog1 = 0; int loi1; void chay1() { if (digitalRead(ch1)==LOW) xung1++; else xung1 ; } void quay1(int value_pwm1) { if (value_pwm1 >=255) { value_pwm1 =255; } if(value_pwm1 0) { digitalWrite( INA1 ,0); digitalWrite( INA2 ,1); analogWrite( pin_pwm,value_pwm1); } if(value_pwm1 < 0) { Sinh viên thực hiện: Vũ Đình Tuân Trần Phạm Quang Việt Hướng dẫn: TS Đặng Phước Vinh 72 Thiết kế robot phun thuốc trừ sâu digitalWrite( INA1 ,1); digitalWrite( INA2 ,0); analogWrite( pin_pwm , -value_pwm1); } } int pid( double loi, double kp , double ki , double kd) { double dloi; static double loitr=0, iloi=0; long temp; dloi = loi - loitr; iloi += loi; if (iloi>=250) { iloi =250; } if (iloi=255) { temp =255; } if(temp