51单片机两轮平衡小车

里面有两轮平衡小车代码，用stc12c5a60s2和mpu6050做的，

#include “STC12C5A60S2.h“
#include “IIC.h“
#include “kaermanlvbo.h“
#include “MPU6050.h“
#include “DIANJI.h“
#define uint unsigned int
#define uchar unsigned char

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 t=0;

void delay（uint z）
{
	uint xy;
	for（x=z;x>0;x--）
		for（y=110;y>0;y--）;
}

void Init_Timer1（void）	//10毫秒@12MHz100Hz刷新频率
{
	AUXR &= 0xBF;		//定时器时钟12T模式
	TMOD&=0x0f;
	TMOD|=0x10;			//设置定时器模式
	TL1 = 0xF0;		//设置定时初值
	TH1 = 0xD8;		//设置定时初值
	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中断
}

void Psn_Calcu（void）	 		 //电机转速和位移值计算
{
	
	speed_r_l =（speed_mr + speed_ml）*0.5;
	speed *= 0.7;		                  //车轮速度滤波
	speed += speed_r_l*0.3;	
	position += speed;	                  //积分得到位移
	if（position<-6000） position = -6000;
	if（position> 6000） position =  6000;	 
}

static float code Kp  = 9;       //PID参数
static float code Kd  = 2.6;	    //PID参数
static float code Kpn = 0.01;      //PID参数
static float code Ksp = 2.0;	    //PID参数

void PWM_Calcu（void）	 			   //电机PWM值计算
{
    
	if（angle<-40||angle>40）               //角度过大，关闭电机 
	{  
	  CCAP0H = 0;
      CCAP1H = 0;
	  return;
	}
	PWM  = Kp*angle + Kd*gyro_y;          //PID：MPU6050角速度和角度
	PWM += Kpn*position + Ksp*speed;      //PID：电机速度和位置
	PWM_R = PWM;
	PWM_L = PWM;
	pwm_dianji（PWM_LPWM_R）;  
}

void main（）
{ 

	delay（500）;	   //上电延时
	init_pwm（）;	       //PWM初始化
    init_timer0（）;     //初始化定时器0，作为PWM时钟源
	Init_Timer1（）;     //初始化定时器1
	Init_Interr（）;     //中断初始化
	init_dianji（）;	   //电机控制初始化
	init_mpu6050（）;
	delay（500）; 	   

	while（1）
	{
	  

	}
}

//********timer1中断***********************

void Timer1_Update（void） interrupt 3
{
  
	TL1 = 0xF0;		//设置定时初值 10ms
	TH1 = 0xD8;		//设置定时初值

	angle_calcu（）;                  //倾角计算
	Psn_Calcu（）;                    //电机位移计算
	PWM_Calcu（）;                    //计算PWM值
	   
	speed_mr = speed_ml = 0;	 
} 


//********右电机中断***********************

void INT_L（void） interrupt 0
{

   if（SPDL == 1）  { speed_ml++; }		 //左电机前进
   else		      { speed_ml--; }		 //左电机后退

 } 


//********左电机中断***********************

void INT_R（void） interrupt 2
{

   if（SPDR == 1）  { speed_mr++; }		 //右电机前进
   else		      { speed_mr--; }		 //右电机后退

 } 			   
	

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-10-26 08:46  小车平衡总程序\
     文件         647  2016-10-26 08:46  小车平衡总程序\DIANJI.h
     文件         667  2016-10-26 08:46  小车平衡总程序\DINGYI.h
     文件        1632  2016-10-26 08:46  小车平衡总程序\IIC.h
     文件         574  2016-10-26 08:46  小车平衡总程序\init.h
     文件        1551  2016-10-26 08:46  小车平衡总程序\kaermanlvbo.h
     文件        1186  2016-10-26 08:46  小车平衡总程序\LCD1206.h
     文件        1201  2016-10-26 08:46  小车平衡总程序\MPU6050.h
     文件         499  2016-10-26 08:46  小车平衡总程序\PWM.h
     文件       17735  2016-10-26 08:46  小车平衡总程序\STC12C5A60S2.h
     文件       25738  2016-10-26 08:46  小车平衡总程序\小车平衡总程序
     文件        2948  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.c
     文件       10750  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.hex
     文件          60  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.lnp
     文件        5945  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.LST
     文件       28326  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.M51
     文件       30412  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.OBJ
     文件      142028  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvgui.Administrator
     文件       74516  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvgui.huahui
     文件      142033  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvgui_Administrator.bak
     文件       74338  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvgui_huahui.bak
     文件        8363  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvopt
     文件       14495  2016-10-26 08:46  小车平衡总程序\小车平衡总程序.uvproj
     文件       58852  2016-10-26 08:46  小车平衡总程序\小车平衡总程序_uvopt.bak
     文件       14491  2016-10-26 08:46  小车平衡总程序\小车平衡总程序_uvproj.bak