??
/***********************************************************************
//?兩輪自平衡車最終版控制程序（6軸MPU6050+互補濾波+PWM電機）?
//?單片機STC12C5A60S2?
//?晶振：20M
//?日期：2012.11.26?-?？
***********************************************************************/

#include?	
#include??????
#include????
#include?

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頭文件



//******角度參數(shù)************

float?Gyro_y;????????//Y軸陀螺儀數(shù)據(jù)暫存
float?Angle_gy;??????//由角速度計算的傾斜角度
float?Accel_x;	?????//X軸加速度值暫存
float?Angle_ax;??????//由加速度計算的傾斜角度
float?Angle;?????????//小車最終傾斜角度
//uchar?value;		?//角度正負極性標記	

//******PWM參數(shù)*************

int???speed_mr;		?//右電機轉(zhuǎn)速
int???speed_ml;		?//左電機轉(zhuǎn)速
int???PWM_R;?????????//右輪PWM值計算
int???PWM_L;?????????//左輪PWM值計算
float?PWM;???????????//綜合PWM計算
//float?PWMI;			?//PWM積分值

//******電機參數(shù)*************

float?speed_r_l;	//電機轉(zhuǎn)速
float?speed;????????//電機轉(zhuǎn)速濾波
float?position;	????//位移

//******藍牙遙控參數(shù)*************
uchar?remote_char;
char??turn_need;
char??speed_need;

//*********************************************************
//定時器100Hz數(shù)據(jù)更新中斷
//*********************************************************

void?Init_Timer1（void）	//10毫秒@20MHz100Hz刷新頻率
{
	AUXR?&=?0xBF;		//定時器時鐘12T模式
	TMOD?&=?0x0F;		//設(shè)置定時器模式
	TMOD?|=?0x10;		//設(shè)置定時器模式
	TL1?=?0xE5;		????//設(shè)置定時初值
	TH1?=?0xBE;		????//設(shè)置定時初值
	TF1?=?0;		????//清除TF1標志
	TR1?=?1;		????//定時器1開始計時
}



//*********************************************************
//中斷控制初始化
//*********************************************************

void?Init_Interr（void）	?
{
	EA?=?1;?????//開總中斷
????EX0?=?1;????//開外部中斷INT0
????EX1?=?1;????//開外部中斷INT1
????IT0?=?1;????//下降沿觸發(fā)
????IT1?=?1;????//下降沿觸發(fā)
	ET1?=?1;????//開定時器1中斷
}



//******卡爾曼參數(shù)************
		
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]?={0000};
float?xdata?PP[2][2]?=?{?{?1?0?}{?0?1?}?};

//*********************************************************
//?卡爾曼濾波
//*********************************************************

//Kalman濾波，20MHz的處理時間約0.77ms；

void?Kalman_Filter（float?Accelfloat?Gyro）		
{
	Angle+=（Gyro?-?Q_bias）?*?dt;?//先驗估計

	
	Pdot[0]=Q_angle?-?PP[0][1]?-?PP[1][0];?//?Pk-先驗估計誤差協(xié)方差的微分

	Pdot[1]=-?PP[1][1];
	Pdot[2]=-?PP[1][1];
	Pdot[3]=Q_gyro;
	
	PP[0][0]?+=?Pdot[0]?*?dt;???//?Pk-先驗估計誤差協(xié)方差微分的積分
	PP[0][1]?+=?Pdot[1]?*?dt;???//?=先驗估計誤差協(xié)方差
	PP[1][0]?+=?Pdot[2]?*?dt;
	PP[1][1]?+=?Pdot[3]?*?dt;
		
	Angle_err?=?Accel?-?Angle;	//zk-先驗估計
	
	PCt_0?=

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????1200??2013-07-23?13:55??MPU6050_Kalman_PWM_remote\DELAY.h

?????文件???????5126??2013-07-24?08:43??MPU6050_Kalman_PWM_remote\I2C.h

?????文件???????2035??2012-11-29?22:35??MPU6050_Kalman_PWM_remote\MOTOR.H

?????文件???????1959??2012-08-04?17:57??MPU6050_Kalman_PWM_remote\MPU6050.H

?????文件???????2429??2013-07-23?00:45??MPU6050_Kalman_PWM_remote\SET_PWM.H

?????文件???????1628??2012-11-26?19:58??MPU6050_Kalman_PWM_remote\SET_SERIAL.H

?????文件????????513??2012-08-13?22:45??MPU6050_Kalman_PWM_remote\STC_ISP.H

?????文件??????29065??2013-09-25?21:07??MPU6050_Kalman_PWM_remote\two_balance_car

?????文件???????8322??2013-07-29?19:35??MPU6050_Kalman_PWM_remote\two_balance_car.c

?????文件??????11336??2013-09-25?21:07??MPU6050_Kalman_PWM_remote\two_balance_car.hex

?????文件?????????62??2013-09-25?21:06??MPU6050_Kalman_PWM_remote\two_balance_car.lnp

?????文件??????15815??2013-09-25?21:06??MPU6050_Kalman_PWM_remote\two_balance_car.LST

?????文件??????33779??2013-09-25?21:07??MPU6050_Kalman_PWM_remote\two_balance_car.M51

?????文件??????34620??2013-09-25?21:06??MPU6050_Kalman_PWM_remote\two_balance_car.OBJ

????.......??????1236??2013-07-21?20:00??MPU6050_Kalman_PWM_remote\two_balance_car.opt.bak

?????文件????????195??2013-09-29?17:46??MPU6050_Kalman_PWM_remote\two_balance_car.plg

????.......??????2193??2013-07-21?10:56??MPU6050_Kalman_PWM_remote\two_balance_car.Uv2.bak

????.......?????62052??2013-09-25?21:15??MPU6050_Kalman_PWM_remote\two_balance_car.uvopt

????.......?????13281??2013-07-22?23:54??MPU6050_Kalman_PWM_remote\two_balance_car.uvproj

?????文件????????872??2013-07-21?10:56??MPU6050_Kalman_PWM_remote\two_balance_car_Opt.Bak

?????文件???????2164??2012-12-01?01:15??MPU6050_Kalman_PWM_remote\two_balance_car_Uv2.Bak

????.......?????62054??2013-09-11?16:55??MPU6050_Kalman_PWM_remote\two_balance_car_uvopt.bak

?????目錄??????????0??2013-09-28?10:22??MPU6050_Kalman_PWM_remote

-----------?---------??----------?-----??----

???????????????291936????????????????????23