IC解密平衡车卡尔曼滤波源码
- //IC解密 两轮自平衡车最终版控制程序（6轴MPU6050+互补滤波+PWM电机）
- // IC解密单片机STC12C5A60S2
- // IC解密晶振：20M
- ***********************************************************************/
- #include <REG52.H>
- #include <math.h>
- #include <stdio.h>
- #include <INTRINS.H>
- typedef unsigned char uchar;
- typedef unsigned short ushort;
- typedef unsigned int uint;
- //******功能模块头文件*******
- #include "DELAY.H" //延时头文件
- #include "STC_ISP.H" //程序烧录头文件
- #include "SET_SERIAL.H" //串口头文件
- #include "SET_PWM.H" //PWM头文件
- #include "MOTOR.H" //电机控制头文件
- #include "MPU6050.H" //MPU6050头文件
- //******角度参数************
- float Gyro_y; //Y轴陀螺仪数据暂存
- float Angle_gy; //由角速度计算的倾斜角度
- float Accel_x; //X轴加速度值暂存
- float Angle_ax; //由加速度计算的倾斜角度
- float Angle; //小车最终倾斜角度
- uchar value; //角度正负极性标记
- //******PWM参数*************
- int speed_mr; //右电机转速
- int speed_ml; //左电机转速
- int PWM_R; //右轮PWM值计算
- int PWM_L; //左轮PWM值计算
- float PWM; //综合PWM计算
- float PWMI; //PWM积分值
- //******电机参数*************
- float speed_r_l; //电机转速
- float speed; //电机转速滤波
- float position; //位移
- //******蓝牙遥控参数*************
- uchar remote_char;
- char turn_need;
- char speed_need;
- //*********************************************************
- //定时器100Hz数据更新中断
- //*********************************************************
- void Init_Timer1(void) //10毫秒@20MHz,100Hz刷新频率
- {
- AUXR &= 0xBF; //定时器时钟12T模式
- TMOD &= 0x0F; //设置定时器模式
- TMOD |= 0x10; //设置定时器模式
- TL1 = 0xE5; //设置定时初值
- TH1 = 0xBE; //设置定时初值
- TF1 = 0; //清除TF1标志
- TR1 = 1; //定时器1开始计时
- }
- //*********************************************************
- //中断控制初始化
- //*********************************************************
- void Init_Interr(void)
- {
- EA = 1; //开总中断
- EX0 = 1; //开外部中断INT0
- EX1 = 1; //开外部中断INT1
- IT0 = 1; //下降沿触发
- IT1 = 1; //下降沿触发
- ET1 = 1; //开定时器1中断
- }
- //******卡尔曼参数************
-
- float code Q_angle=0.001;
- float code Q_gyro=0.003;
- float code R_angle=0.5;
- float code dt=0.01; //dt为kalman滤波器采样时间;
- char code C_0 = 1;
- float xdata Q_bias, Angle_err;
- float xdata PCt_0, PCt_1, E;
- float xdata K_0, K_1, t_0, t_1;
- float xdata Pdot[4] ={0,0,0,0};
- float xdata PP[2][2] = { { 1, 0 },{ 0, 1 } };
- //*********************************************************
- // 卡尔曼滤波
- //*********************************************************
- //Kalman滤波，20MHz的处理时间约0.77ms；
- void Kalman_Filter(float Accel,float Gyro)
- {
- Angle+=(Gyro - Q_bias) * dt; //先验估计
-
- Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
- Pdot[1]=- PP[1][1];
- Pdot[2]=- PP[1][1];
- Pdot[3]=Q_gyro;
-
- PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
- PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
- PP[1][0] += Pdot[2] * dt;
- PP[1][1] += Pdot[3] * dt;
-
- Angle_err = Accel - Angle; //zk-先验估计
-
- PCt_0 = C_0 * PP[0][0];
- PCt_1 = C_0 * PP[1][0];
-
- 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 * PP[0][1];
- PP[0][0] -= K_0 * t_0; //后验估计误差协方差
- PP[0][1] -= K_0 * t_1;
- PP[1][0] -= K_1 * t_0;
- PP[1][1] -= K_1 * t_1;
-
- Angle += K_0 * Angle_err; //后验估计
- Q_bias += K_1 * Angle_err; //后验估计
- Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
- }
芯片解密
