基于STC12C5A60S2两轮平衡小车程序源码

基于STC12C5A60S2单片机的两轮平衡小车程序源码，是我为电子设计比赛做的，采用陀螺仪MPU6050及卡尔曼滤波、PID控制，小车平衡效果自己测试便知，代码通俗易懂，希望帮助到大家！

#include 
#include “mpu6050.h“


//******卡尔曼滤波***********
float xdata Q_angle=0.001;//0.001  
float xdata Q_gyro=0.03;//0.03
float xdata R_angle=0.5;//0.5
float xdata dt=0.01;//0.1	                  //dt为kalman滤波器采样时间;
char  xdata 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] ={0000};
float xdata PP[2][2] = { { 1 0 }{ 0 1 } };
//float Angle_EndGyro_End;			//小车最终倾斜角度，及角速度

/*卡尔曼函数*/
void Kalman_Filter（float Accelfloat Gyro）		
{
	Angle_End +=（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_End;	//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_End	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
  Gyro_End = Gyro - Q_bias;	 //输出值（后验估计）的微分=角速度

}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2015-05-29 10:27  小车平衡稳定\
     文件        1446  2015-04-28 10:29  小车平衡稳定\Kalman.c
     文件         103  2015-03-28 02:25  小车平衡稳定\Kalman.h
     目录           0  2015-05-29 10:27  小车平衡稳定\Listings\
     文件        3266  2015-04-28 10:30  小车平衡稳定\Listings\Kalman.lst
     文件        2746  2015-04-28 10:32  小车平衡稳定\Listings\Spe_Pos.lst
     文件       39337  2015-05-29 10:27  小车平衡稳定\Listings\chengxu.m51
     文件        6183  2015-03-30 21:47  小车平衡稳定\Listings\lcd1602.lst
     文件        5798  2015-05-29 10:27  小车平衡稳定\Listings\main.lst
     文件        9081  2015-03-30 21:47  小车平衡稳定\Listings\mpu6050.lst
     文件        1670  2015-04-28 10:31  小车平衡稳定\Listings\pid_own.lst
     文件        4794  2015-04-28 10:30  小车平衡稳定\Listings\pwm_motor.lst
     目录           0  2015-05-29 10:27  小车平衡稳定\objects\
     文件         122  2015-04-28 10:30  小车平衡稳定\objects\Kalman.__i
     文件       17328  2015-04-28 10:30  小车平衡稳定\objects\Kalman.obj
     文件         125  2015-04-28 10:32  小车平衡稳定\objects\Spe_Pos.__i
     文件       12596  2015-04-28 10:32  小车平衡稳定\objects\Spe_Pos.obj
     文件      109213  2015-05-29 10:27  小车平衡稳定\objects\chengxu
     文件       12145  2015-03-19 00:22  小车平衡稳定\objects\chengxu.LIB
     文件         138  2015-03-19 00:22  小车平衡稳定\objects\chengxu.__b
     文件         987  2015-05-29 10:27  小车平衡稳定\objects\chengxu.build_log.htm
     文件       15843  2015-05-29 10:27  小车平衡稳定\objects\chengxu.hex
     文件         252  2015-05-29 10:27  小车平衡稳定\objects\chengxu.lnp
     文件       12758  2015-03-30 21:47  小车平衡稳定\objects\lcd1602.obj
     文件       24956  2015-05-29 10:27  小车平衡稳定\objects\main.obj
     文件       22037  2015-03-30 21:47  小车平衡稳定\objects\mpu6050.obj
     文件         125  2015-04-28 10:31  小车平衡稳定\objects\pid_own.__i
     文件       12096  2015-04-28 10:31  小车平衡稳定\objects\pid_own.obj
     文件         131  2015-04-28 10:30  小车平衡稳定\objects\pwm_motor.__i
     文件       14441  2015-04-28 10:30  小车平衡稳定\objects\pwm_motor.obj
     文件       46221  2010-07-27 17:31  小车平衡稳定\STC12C5A.h
............此处省略15个文件信息