两轮平衡车STC12单片机

两轮平衡车代码，，参考zstone代码修改，蓝牙控制

								  
/***********************************************************************
// 两轮自平衡车最终版控制程序（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 =

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

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