VI ĐIỀU KHIỂN BÀI TẬP LỚN THIẾT KẾ XE CÂN BẰNG

HO CHI MINH CITY UNIVERSITY OF TECHNOLOGY VI ĐIỀU KHIỂN BÀI TẬP LỚN: THIẾT KẾ XE CÂN BẰNG Class: L02 Group 6: Trịnh Trần Vĩnh Ân - 1912645 Nguyễn Quốc Khánh - 1913746 Lâm Hiệp Hưng - 1910227 Trần Hoàng Anh Tuấn - 1915789 Phạm Trần Bá Ngọc – 1914357 Phạm Cao Tâm – 1915029 Hà Như Thuần - 1915371 Lecturer: TS Đoàn Thế Thảo Ho Chi Minh City University of Technology Faculty of Mechanical Engineering LỜI NÓI ĐẦU Trong thời đại cơng nghệ tự động hóa trao đổi liệu 4.0, việc đưa vào thiết bị trường điều khiển tự động ngày phổ biến nhanh chóng Các nhà doanh nghiệp khơng muốn tối ưu hóa khơng gian sản xuất mà muốn giảm thiểu tác động người, tăng hiệu suất lao động đơn vị thời gian Các thiết bị, cấu chấp hành mà phải có thuộc tính tự động, lại phải vừa nhỏ gọn, tiện lợi, dễ giám sát Xe hai bánh tự cân (xe tự hành) mơ hình áp dụng rộng nhiều quốc gia giới Xe sử dụng để phục vụ nhu cầu giao thông người (tức người điều khiển) với nhiều chủng loại Selfbalancing hoverboard (ván trượt) scooter (xe điện) Trong nhà kho cơng nghiệp, vận chuyển hàng hóa phải dùng tới xe bánh kéo container hàng với kích thước lớn, di chuyển mặt phẳng nhiều lúc phải cần tới không gian rộng để thực thao tác xoay chuyển hướng, đơi thân người phải tự tay điều khiển vận hành chúng Như tiêu chí tối ưu hóa khơng gian giảm thiểu nhân công chưa thực giải triệt để Một ý tưởng trước mắt chúng ta: đưa mơ hình xe tự hành vào vận tải, di chuyển khu công nghiệp Không cho nhân viên di chuyển hay xe bánh tự động mang hàng Amazon thực hiện, mà xe tự hành mang hàng chúng Các mơ hình xe tự cân bằng, tự xoay chỗ, mang tải khối lượng lớn, cao lập trình cho cặp xe mang khối tải dài theo quĩ đạo cho trước Như khơng gian dành cho phương tiện vận chuyển hơn, người cần phụ trách công việc điều khiển với giám sát Ý tưởng có, việc đưa ý tưởng thành thực tế ta cần phải giải tốn • Thiết kế phần cứng cho xe Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering • Thiết lập cho xe tự cân • Thiết lập cho xe di chuyển • Thiết lập cho xe di chuyển theo quĩ đạo cho trước • Thiết lập cho nhiều xe mang tải di chuyển theo quĩ đạo cho trước Tới có nhiều báo đề xuất hàng loạt phương án để giải toán Trong qui mơ mơn học trình độ sinh viên năm 3, nhóm xin phép tìm hiểu mơ hình xe tự hành anh chị khóa trước gửi lại Mơ hình thỏa mãn hai yêu cầu nêu Nhóm nghiên cứu hy vọng qua kì học tìm hiểu phân tích, bạn sinh viên khơng có kiến thức định thành phần mơ hình, thực hành phần lý thuyết mơn Vi điều khiển mà cịn có kĩ làm việc với mạch điều khiển, lập trình nạp cho vi xử lý… Bài báo cáo sau đề cập tới cấu trúc phần cứng, phần lý thuyết cần tìm hiểu thêm, tìm thuật tốn lập trình cho vi điều khiển Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Mô hình xe nhóm Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Mục lục I MỤC TIÊU THIẾT KẾ: II PHẦN CỨNG CỦA XE Kit pic Vi điều khiển PIC16F877AA Mô-đun L298 Cảm biến đo góc MPU6050 Động cơ: 10 III CHƯƠNG TRÌNH ERROR! BOOKMARK NOT DEFINED TÀI LIỆU THAM KHẢO…………………………………………………………………… Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering I Mục tiêu thiết kế: Đọc cảm biến từ MPU6050 thông qua giao thức I2C với MPU6050 slave, MCU master Từ liệu nhận điều khiển động DC vàng di chuyển thông qua driver L298 Sơ đồ mạch: 5𝑉 𝑆𝐶𝐿 𝑆𝐷𝐴 𝑀𝑃𝑈6050 5𝑉 𝑆𝐶𝐿 𝑆𝐷𝐴 𝑀𝐶𝑈 𝑂𝑢𝑡1 𝐼𝑛1 𝐴0 𝐼𝑛2 𝐸𝑛𝐴 𝐴1 𝑅𝐶2(𝑃𝑊𝑀) 𝑀𝑜𝑡𝑜𝑟 𝐿298 𝑂𝑢𝑡2 𝑉𝑖𝑛 𝑉𝑖𝑛 5𝑉 𝑜𝑢𝑡 𝑃𝐼𝑁 + − II PHẦN CỨNG CỦA XE Xe tự cân bao gồm: • • • • Mơ-đun L298 Vi điều khiển PIC16F4550 Cảm biến đo gia tốc MPU6050 Pin Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering • Kit pic • Động Kit pic Kit pic Vi điều khiển PIC16F877A PIC16F877A Vi điều khiển PIC 40 chân sử dụng hầu hết dự án ứng dụng nhúng Nó có năm cổng cổng A đến cổng E Nó có ba định thời có định thời bit định thời 16 Bit Nó hỗ trợ nhiều giao thức giao tiếp giao thức nối tiếp, giao thức song song, giao thức I2C PIC16F877A hỗ trợ ngắt chân phần cứng ngắt định thời Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering PIC16F877A Sơ đồ chân Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Sơ đồ chân Mô-đun L298 Mạch cầu H - L298 mạch điểu khiển hai động cách sử dụng đóng ngắt liên tục Điện áp cấp cho mạch 7V – 12V, module cho đầu 5V điện áp đầu vào lớn 7V Jumper 5V kích hoạt • • • • Công suất 20W T = 75𝑜 𝐶 Module gồm có Mạch cầu H với chân Cổng Enable Motor A, Cổng Enable Motor B với chân/cổng Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering • Các chân tín hiệu điều khiển Motor A Motor B, Motor chân tín hiệu • Cổng Motor A Motor B, cổng có chân để nối dây sang động • Cụm cổng nguồn, bao gồm chân 7-12V Input, chân đất GND, chân 5V Output L298 Ta dùng bảng logic cho chân tín hiệu điều khiển Bảng Logic Cảm biến đo góc MPU6050 Module cảm biến gia tốc MPU-6050 GY-521 tích hợp gia tốc trục với quay hồi chuyển trục giúp kiểm soát cân định hướng chuyển động cho robot, máy bay, drone, tay cầm chơi game, hệ thống giữ thăng cho camera/máy ảnh, nhận biết rơi, rung, lấc,… • Chip: MPU-6050 ( 16bit ADC, 16bit data out ) Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering • • • • Giá trị Gyroscopes khoảng: +/- 250 500 1000 2000 degree/sec Giá trị Acceleration khoảng: +/- 2g, +/- 4g, +/- 8g, +/- 16g Giao tiếp: I2C Nguồn sử dụng: 3V - 5V (DC MPU6050 cảm biến trục bao gồm trục gia tốc kế Acceleration ( ax, ay, a¬z )g trục quay hồi chuyển Gyroscopes( gx, gy, gz ¬) degree/sec Hoạt động với 3.3V giao tiếp I2C với tốc độ tối đa 400kHz Các cảm biến bên MPU6050 sử dụng chuyển đổi tương tự-số ( Analog to Digital Conventer-ADC) 16 bit cho kết chi tiết góc quay, tọa độ… Với 16 bit có 216 = 655362 giá trị cho cảm biến Tùy thuộc vào yêu cầu bạn, cảm biến MPU6050 hoạt động chế độ tốc độ xử lí cao chế độ đo góc quay xác Hơn MPU6050 có sẵn đệm liệu 1-24byte cho phép vi điều khiển phát lệnh cho cảm biến nhận liệu sau MPU6050 tính tốn xong Cấu trúc chân pin cảm biến MPU6050: • • • • • • • • VCC 5V/3V3 GND 0V SCL chân phát xung clock tromg giao tiếp I2C SDA chân phát xung data giao tiếp I2C XDA chân liệu ( kết nối với cảm biến khác) XCL chân xung ( kết nối với cảm biến khác) AD0 bit0 địa I2C INT chân ngắt Dữ liệu nhận giá trị trục từ cảm biến MPU6050 Vi điều khiển P a g e | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Đo góc nghiêng Accelerometer: MPU6050 có gia tốc kế trục quay hồi chuyển trục Gia tốc kế đo gia tốc dọc theo ba trục quay hồi chuyển đo tốc độ góc ba trục Để đo góc nghiêng robot, cần giá trị gia tốc dọc theo trục y z Hàm atan2 (y, z) cho biết góc có đơn vị radian trục z dương mặt phẳng điểm cho tọa độ (z, y) mặt phẳng Cơng thức tính góc: 𝑎𝑐𝑐𝑌 𝛷 = 𝑎𝑟𝑐𝑡𝑎𝑛 ( ) 𝑎𝑐𝑐𝑍 Động cơ: Thơng số kỹ thuật: • +Điện áp hoạt động:3V~ 9V DC (Hoạt động tốt từ - 8V) • +208 Vịng/ Phút (5V) • +Tốc độ khơng tải: 125 Vịng/ Phút (3V) Vi điều khiển P a g e 10 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Động vàng giảm tốc III CHƯƠNG TRÌNH #include #FUSES NOWDT, PUT,HS,NOPROTECT,NOLVP #USE DELAY(CLOCK=20M) #use i2c(MASTER,SCL=PIN_C3,SDA=PIN_C4) #include #use TIMER(TIMER=1, TICK=1ms, BITS=16, NOISR) #include "MPU6050.c" #include "Kalman.h" #define RAD_TO_DEG 180/PI float Ax, Az, goc, result ; Vi điều khiển P a g e 11 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering float i=0, error, last_error=0,output,dt1,it,pt,gyroy; UNSIGNED int16 timerh; int16 speed; float kp = 29 ; float ki = 0; float kd =10 ; void pid () { error=-2.2-result; pt=kp*error; i=i+error; if(i>100) { i=100; } else if(i1000) { output=1000; } else if(output0) { speed=(int16)output; output_high(PIN_A2); output_low(PIN_A3); set_pwm1_duty(speed); Vi điều khiển P a g e 14 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering } else { speed=-(int16)output; output_high(PIN_A3); output_low(PIN_A2); output_high(PIN_A1); output_low(PIN_A0); set_pwm1_duty(speed); } } void motorl() { if(output>0) { speed=(int16)output; output_high(PIN_A0); Vi điều khiển P a g e 15 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering output_low(PIN_A1); set_pwm2_duty(speed); } else { speed=-(int16)output; output_high(PIN_A1); output_low(PIN_A0); set_pwm2_duty(speed); } } void dungr() { set_pwm1_duty(0); } void dungl() Vi điều khiển P a g e 16 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering { set_pwm2_duty(0); } void main() { set_tris_c(0x00); set_tris_a(0x00); SETUP_TIMER_2(T2_DIV_BY_16,249,1); setup_ccp1(CCP_PWM); setup_ccp2(CCP_PWM); MPU6050_init(); while(true) { Vi điều khiển P a g e 17 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering Ax=MPU6050_get_Ax(); Az= MPU6050_get_Az(); gyroy=MPU6050_get_Gy(); set_ticks(0); goc=(atan2(Ax,Az))*RAD_TO_DEG; result=kalmanCalculate(goc,gyroy,get_ticks()-timerh); timerh=get_ticks(); IF((result-40)) { pid(); motorr(); motorl(); } else { dungr(); dungl(); Vi điều khiển P a g e 18 | 21 Ho Chi Minh City University of Technology Faculty of Mechanical Engineering } } } TÀI LIỆU THAM KHẢO https://www.dientuhello.com/suu-tam-huong-dan-lam-robot-2-banh-tu-can-bang/ https://vidieukhien.xyz/2014/10/08/cam-bien-6-truc-gia-toc-goc-mpu-6050-gy521giao-tiep-i2c-voi-pic-choi-game-asphalt-8ccscode/?fbclid=IwAR1xJW56YndvcWO0XvOQXNJKqOBu_rvU96fIa4a6Pfe4sLi8nP 36-JszFwo Vi điều khiển P a g e 19 | 21