??
/***********************************************************************
//?兩輪自平衡車最終版控制程序（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頭文件



//******角度參數************

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毫秒@20MHz100Hz刷新頻率
{
	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]?={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-先驗估計誤差協方差的微分

	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

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-05-12?16:15??51平衡車\
?????目錄???????????0??2016-05-12?16:14??51平衡車\PID\
?????文件??????196259??2015-11-08?21:17??51平衡車\PID\位置PID.pdf
?????文件??????175420??2014-08-26?10:18??51平衡車\PID\關于PID.pdf
?????文件??????219348??2016-03-17?19:43??51平衡車\PID\基于PID算法和89C52單片機的溫度控制系統_張艷艷.pdf
?????文件??????426223??2016-03-17?19:44??51平衡車\PID\基于單片機的PID溫度控制系統設計_蒙楠.pdf
?????文件??????408197??2016-03-17?19:43??51平衡車\PID\基于數字PID和89C52單片機的溫度控制系統_劉迪.pdf
?????文件??????448607??2016-02-03?09:39??51平衡車\PID\平衡小車調試指南（直立環_速度環）.pdf
?????目錄???????????0??2016-05-12?15:58??51平衡車\兩輪平衡車參考方案\
?????目錄???????????0??2016-05-12?15:58??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\
?????目錄???????????0??2016-05-12?15:59??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\
?????文件??????113255??2013-04-26?21:03??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\004503oom50fg2mgmf8u5q.jpg
?????文件??????110666??2013-04-26?21:02??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\004507k9opo9cooy19o19x.jpg
?????文件??????108070??2013-04-26?21:02??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\004510g8uainfz0kxklhz3.jpg
?????文件???????33816??2013-04-19?22:31??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\1363242342.jpg
?????文件???????39508??2013-04-19?22:31??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\1363242359?（1）.jpg
?????文件???????41217??2013-04-19?22:32??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\1363242373.jpg
?????文件???????57103??2013-04-19?22:32??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\1363242394.jpg
?????文件?????1370713??2013-04-26?20:30??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\BT_LHcar.apk
?????文件?????1364732??2013-04-26?20:31??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\BT_LHcar.zip
?????目錄???????????0??2016-05-12?16:00??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\
?????文件????????1204??2012-07-25?22:27??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\DELAY.h
?????文件????????5002??2012-11-26?19:53??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\I2C.h
?????文件????????2035??2012-11-29?22:35??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\MOTOR.H
?????文件????????1959??2012-08-04?17:57??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\MPU6050.H
?????文件????????2435??2012-11-28?20:05??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\SET_PWM.H
?????文件????????1628??2012-11-26?19:58??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\SET_SERIAL.H
?????文件?????????513??2012-08-13?22:45??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\STC_ISP.H
?????文件???????29280??2012-12-01?21:20??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\two_balance_car
?????文件???????15720??2012-12-01?21:20??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\two_balance_car.LST
?????文件???????33972??2012-12-01?21:20??51平衡車\兩輪平衡車參考方案\4、?兩輪平衡車參考方案\兩輪小米重力感應小車\MPU6050_Kalman_PWM_remote\two_balance_car.M51
............此處省略233個文件信息