33 Chương TRIỂN KHAI MÔ HÌNH 7.1 Sơ đồ tổng quát hệ thống: Hình 7.1: Sơ đồ tổng quát 7.2 Sơ đồ mạch điện: Hình 7.2: Sơ đồ nguyên lý mạch điện 34 7.3 Động cơ: Điện áp hoạt động: 12 [v] Tốc độ motor 8100 [RPM] Tốc độ sau giam tốc 120 [RPM] Tỉ số giảm tốc 64:1 Dòng tải tối đa 1400 [mA] Dòng không tải 74 [mA] Momen xoắn 1.72 [Nm] Độ phân giải Encoder 12 [Xung/vòng] Bảng 7.1: Các thông số kỹ thuật động Hình 7.3: Động sử dụng mô hình 7.4 Cảm biến MPU6050 : Điện áp hoạt động: [v] Giao tiếp I2C Giải đo Gyroscopes ±250, 500, 1000, 2000 [Deg/s] Giải đo Acceleration ± 2, 4, 8, 16 [g] Bảng 7.2: Các thông số kỹ thuật cảm biến vận tốc góc gia tốc góc 35 Hình 7.4: Modul cảm biến vận tốc góc gia tốc góc 7.5 Modul khiển động LMD18200: Điện áp hoạt động Vmax: 55 [v] Dòng điện làm việc Imax [A] RDS on 0.33[Ω] Bảng 7.3: Các thông số kỹ thuật Modul điều khiển động Hình 7.5: Modul điều khiển động 36 7.6 Bánh xe: Đường kính bánh xe: 95 [mm] Đường kính trục: [mm] Khối lượng tải: 100 [kg] Bảng 7.4: Các thông số kỹ thuật bánh xe Hình 7.6: Bánh xe 37 7.7 Mô hình thực tế: Hình 7.7: Mô hình thực tế 7.8 Phần mềm: #include #include #include #include #include #include #include #define g 9.81 #define L 0.13 #define MW 0.19933 38 #define R 0.048 #define MB (0.83053 + 0.620) #define D 0.21 #define k1 20 #define KS 350 float f1(float x1) { float temp1, temp2; temp1 = ((-0.75 * g * sin(x1))/L); temp2 = (((0.75 * cos(x1) * ((Mw * R) + (MB * L * cos(x1)))) / (((2 * Mw) + MB) * L)) - 1); return(temp1/temp2); } float f2(float x1,x2) { float temp1, temp2; temp1 = (( 0.75 * MB * sin(x1) * cos(x1) * x2 * x2)/ ((2* Mw)+ MB)); temp2 = (((0.75* cos(x1)* ((Mw* R)+ (MB* L* cos(x1)))) / (((2* Mw) + MB)* L))- 1); return(temp1/temp2); } float g1(float x1) { float temp1, temp2; temp1 = (((0.75 * (1 + (sin(x1) * sin(x1)))) / (MB * L * L)) + ((0.75 * cos(x1))/(((2 * Mw) + MB) * R * L))); temp2 = (((0.75* cos(x1)* ((Mw* R)+ (MB* L* cos(x1)))) / (((2* Mw) + MB)* L))- 1); return(temp1/temp2); 39 } float SMC(float x1,x2, ref) { float temp1, temp2, err,s,u ,r_dotdot; err = ref - x1; temp1 = (err - err_old)/d_t ; // e_dot temp2 = (ref - r_old)/d_t; //r_dot r_dotdot = (temp2 - r_dotdot_old)/d_t; s = (k1*err)+temp1; if (s > 0.5) s = 0.5; else if(s < -0.5) s = -0.5; s = s * KS; u = ((k1*temp1) + s + r_dotdot - f1(x1) - f2(x1,x2) )/g1(x1); r_dotdot_old = r_dotdot; err_old = temp1; r_old = ref; return(u); } float pole_palacement(float ek) { float uk = (0.0158224*ek)-(0.01276*ek_1)+(0.12*uk_1); uk_1 = uk; ek_1 = ek; return(uk); } Void main(void) { printf("Startup \r\n"); 40 set_pullup(TRUE,PIN_B4); //QEA set_pullup(TRUE,PIN_B5); //QEB setup_qei(QEI_MODE_X4|QEI_TIMER_INTERNAL,QEI_FILTER_DIV_16,Q EI_FORWARD); //| QEI_SWAP_AB control_slave(QEI_4); qei_set_count(0x0000); control_slave(reset); //init motor printf("Init motor modul \r\n"); motor_init(); printf("Init MPU6050 modul "); if(InitMpu6050() != 0x68) {printf("Sensor not correct\r\n"); while(true) restart_wdt(); } else printf("Ok\r\n"); printf("Init Kalman \r\n"); init_kalman(); printf("Go \r\n"); delay_ms(500); enable_interrupts(INT_RDA); enable_interrupts(INTR_GLOBAL); TICK_TYPE CurrentTick,PreviousTick; unsigned int8 on_off = on; float datadeg[10], deg_sum = 0; unsigned int8 i; for(i = 0;i 0) {left_forward; pwm_l = (unsigned int16)calcutlate_pwm(c_left);} else {left_revert; pwm_l = (unsigned int16) calcutlate_pwm(-c_left);} if(c_right > 0) { right_forward; pwm_r = (unsigned int16) calcutlate_pwm(c_right);} else { right_revert; pwm_r = (unsigned int16) calcutlate_pwm(-c_right);} set_motor_pwm_duty(1,motor_left,pwm_l); set_motor_pwm_duty(1,motor_right,pwm_r); restart_wdt(); } } 42 7.9 Lưu đồ giải thuật: Start Khởi tạo biến; Cài đặt timer, interrupt; Cập nhật Timer (dT); Đọc góc nghiêng θ, δ Kalman filter Cθ = SMC() Cδ = Pole() Decoupling CLeft, CRight Read ADC() Calculate PWM Motor control Hình 7.8: Lưu đồ giải thuật chương trình 43 7.10 Kết thực mô hình: Mô hình hoạt động điều khiển tín hiệu RF theo giao thức truyền nối tiếp RS232 Mô hình đáp ứng thao tác cần thiết di chuyển phía trước ngược lại, chuyển hướng rẽ trái, phải theo người điều khiển Tuy nhiên hiện, tượng dao động (chattering) xảy xe đứng yên vị trí cân bằng( θ = [Rad])  ... điều khiển động 36 7.6 Bánh xe: Đường kính bánh xe: 95 [mm] Đường kính trục: [mm] Khối lượng tải: 100 [kg] Bảng 7.4: Các thông số kỹ thuật bánh xe Hình 7.6: Bánh xe 37 7.7 Mô hình thực tế: Hình... Modul khiển động LMD18200: Điện áp hoạt động Vmax: 55 [v] Dòng điện làm việc Imax [A] RDS on 0.33[Ω] Bảng 7.3: Các thông số kỹ thuật Modul điều khiển động Hình 7.5: Modul điều khiển động 36 7.6 Bánh. .. trước ngược lại, chuyển hướng rẽ trái, phải theo người điều khiển Tuy nhiên hiện, tượng dao động (chattering) xảy xe đứng yên vị trí cân bằng( θ = [Rad])