??
/***********************************************************************
//?兩輪自平衡車最終版控制程序（6軸MPU6050+互補(bǔ)濾波+PWM電機(jī)）?
//?單片機(jī)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“			//電機(jī)控制頭文件
#include?“MPU6050.H“		//MPU6050頭文件



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

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

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

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

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

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

//******藍(lán)牙遙控參數(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標(biāo)志
	TR1?=?1;		????//定時器1開始計(jì)時
}



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

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;?//先驗(yàn)估計(jì)

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

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

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

?????文件?????113255??2013-04-26?21:03??兩輪小米重力感應(yīng)小車\004503oom50fg2mgmf8u5q.jpg

?????文件?????110666??2013-04-26?21:02??兩輪小米重力感應(yīng)小車\004507k9opo9cooy19o19x.jpg

?????文件?????108070??2013-04-26?21:02??兩輪小米重力感應(yīng)小車\004510g8uainfz0kxklhz3.jpg

?????文件??????33816??2013-04-19?22:31??兩輪小米重力感應(yīng)小車\1363242342.jpg

?????文件??????39508??2013-04-19?22:31??兩輪小米重力感應(yīng)小車\1363242359?（1）.jpg

?????文件??????41217??2013-04-19?22:32??兩輪小米重力感應(yīng)小車\1363242373.jpg

?????文件??????57103??2013-04-19?22:32??兩輪小米重力感應(yīng)小車\1363242394.jpg

?????文件????1370713??2013-04-26?20:30??兩輪小米重力感應(yīng)小車\BT_LHcar.apk

?????文件????1364732??2013-04-26?20:31??兩輪小米重力感應(yīng)小車\BT_LHcar.zip

?????文件???????1204??2012-07-25?22:27??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\DELAY.h

?????文件???????5002??2012-11-26?19:53??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\I2C.h

?????文件???????2035??2012-11-29?22:35??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\MOTOR.H

?????文件???????1959??2012-08-04?17:57??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\MPU6050.H

?????文件???????2435??2012-11-28?20:05??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\SET_PWM.H

?????文件???????1628??2012-11-26?19:58??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\SET_SERIAL.H

?????文件????????513??2012-08-13?22:45??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\STC_ISP.H

?????文件??????29280??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car

?????文件???????8317??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.c

?????文件??????11350??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.hex

?????文件?????????62??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.lnp

?????文件??????15720??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.LST

?????文件??????33972??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.M51

?????文件??????34868??2012-12-01?21:20??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.OBJ

?????文件????????873??2013-04-26?20:58??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.Opt

?????文件????????213??2013-04-26?20:58??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.plg

?????文件???????2193??2013-04-26?20:58??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car.Uv2

?????文件???????1190??2012-12-01?21:18??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car_Opt.Bak

?????文件???????2164??2012-12-01?01:15??兩輪小米重力感應(yīng)小車\MPU6050_Kalman_PWM_remote\two_balance_car_Uv2.Bak

????..A.SH.???????162??2013-04-19?22:46??兩輪小米重力感應(yīng)小車\~$063829837731.pdf

?????文件??????48680??2013-04-19?22:32??兩輪小米重力感應(yīng)小車\下位機(jī).rar

............此處省略30個文件信息