自制两轮平衡车.ppt

1、自制两轮平衡车,制作：朱兆丰,控制器：ATmega16；8MHz；加速度传感器：MMA2260；陀螺仪：EWTS82；传感器的融合：卡尔曼滤波；马达：EN_2342CR（速比64）+双路12脉冲编码器+CD40106对信号整形；驱动板芯片：CD4001+IR2111+IRF1404（驱动电流可以很大）；制作资料在压缩包里面，供参考；,速度传感器是mma7361 陀螺仪是ENC-03包邮第八届 飞思卡尔智能车MC9S12XS128MAA 80脚单片机最小系统板,/卡耳漫滤波 PD融合控制代码:#include Wire.h#include I2Cdev.h#include MPU6050.hMP

2、U6050 accelgyro;/#include ;/LiquidCrystal lcd( 12, 11, 10, 9, 8,7);#define Gry_offset -20 / 陀螺仪偏移量/#define Gyr_Gain 0.04 / 满量程2000dps时灵敏度(dps/digital)#define Gyr_Gain 65.5#define pi 3.14159int16_t ax, ay, az;int16_t gx, gy, gz;,/* 互补滤波器参数 */unsigned long preTime = 0; / 采样时间/float f_angle = 0.0; / 滤波

3、处理后的角度值,* PID控制器参数 */unsigned long lastTime;float Output; /;, Setpoint,Input;/double errSum, lastErr;float kp, ki, kd,kpp;/int SampleTime = 0.1; /1 sec/float Outputa = 0.0;float angleA,omega;/double Kp, Ki, Kd;float P22 = 1, 0 , 0, 1 ;float Pdot4 = 0,0,0,0;static const double Q_angle=0.001, Q_gyro=0

4、.003, R_angle=0.5,dtt=0.007,C_0 = 1;float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;float angle, angle_dot; / aaxdot,aax;float position_dot,position_dot_filter,positiono;/double Speed_Need=0;/float K_angle=2;/float K_angle_dot=0.5; /换算系数：256/10 =25.6；/float K_position=0.1; /换算系数：(256/10

5、) * (2*pi/(64*12)=0.20944；/256/10:电压换算至PWM，256对应10V； /float K_position_dot=1; /换算系数：(256/10) * (25*2*pi/(64*12)=20.944；/-,int oommm;int ooommm;/int oooommmm;/double OLH= 10,ORH = 10;int TN1=22;int TN2=23;int TN3=24;int TN4=25;int ENA=2;int ENB=3;/-void setup() Wire.begin();/lcd.begin(16, 2);/lcd.pri

6、nt(hello, world!);/delay(1000);,accelgyro.initialize(); delay(500);pinMode(22,OUTPUT); pinMode(23,OUTPUT); pinMode(24,OUTPUT); pinMode(25,OUTPUT); pinMode(2,OUTPUT); pinMode(3,OUTPUT); delay(100);/Serial.begin(9600);void loop() accelgyro.getMotion6(,/-angleA= atan2(ay , az) * 180 / pi-0.2;/ 根据加速度分量得

7、到的角度(degree)/180度至0至-180（360度）取0度为坚直时中立点 因为坚直时为有偏差，所以减去0.2度./omega=Gyr_Gain * (gx +Gry_offset); / 当前角速度(degree/s) omega=(gx +Gry_offset)/Gyr_Gain; / 当前角速度(degree/s)if (abs(angleA)30) /这个是误差达到一定程度后的系统关闭开关.Kalman_Filter(angleA,omega);PIDD();PWMB();/LCDD(); else analogWrite(ENA, 0); /PWM调速a=0-255analog

8、Write(ENB, 0); delay(10);/=-,void Kalman_Filter(double angle_m,double gyro_m)angle+=(gyro_m-q_bias) * dtt;Pdot0=Q_angle - P01 - P10;Pdot1=- P11;Pdot2=- P11;Pdot3=Q_gyro;P00 += Pdot0 * dtt;P01 += Pdot1 * dtt;P10 += Pdot2 * dtt;P11 += Pdot3 * dtt;angle_err = angle_m - angle;PCt_0 = C_0 * P00;PCt_1 = C

9、_0 * P10;E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * P01;P00 -= K_0 * t_0;P01 -= K_0 * t_1;P10 -= K_1 * t_0;P11 -= K_1 * t_1;angle+= K_0 * angle_err;q_bias += K_1 * angle_err;angle_dot = gyro_m-q_bias;/也许应该用last_angle-angle/-,voidPIDD() kp=analogRead(8)*0.01; ki

10、=analogRead(9)*0.007; kd=analogRead(10)*0.007; kpp=analogRead(11)*0.007;/aaxdot=0.5*aaxdot+0.5*angle_dot;/aax=0.5*aax+0.5*angle; position_dot=Output*0.04; /利用PWM值代替轮速传感器的信号 position_dot_filter*=0.9; /车轮速度滤波 position_dot_filter+=position_dot*0.1; positiono+=position_dot_filter; /positiono+=Speed_Need

11、; positiono=constrain(positiono,-500,500);Output= 2*angle *kp + 0.5*angle_dot*ki +0.02*positiono *kd + 1*position_dot_filter *kpp; /Output= K_angle*angle *kp + K_angle_dot*angle_dot *ki +K_position*positiono *kd + K_position_dot*position_dot_filter *kpp;/-电机正反转 PWM输出-void PWMB()if(Output0) digitalWrite(TN1, HIGH); digitalWrite(TN2, LOW); digitalWrite(TN3, HIGH); digitalWrite(TN4, LOW);,if(Output0) digitalWrite(TN1