51做平衡车

其实程序过程很简单，先是采集传感器MPU6050的数据，并进行加工，获得当前小车倾斜角，然后根据此角度做PID调节，得到小车两个电机的PWM脉宽，调整轮子速度，使之回到倾角为0的状态，即保持平衡。然后就不停地重复采集->处理->调节->处理这一过程。 在此基础上，附加两电机的PWM值，即可实现前进，倒退，左转，右转动作

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