基于51单片机的平衡车程序

#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 x,y; for(x=z;x>0;x--) for(y=110;y>0;y--); } void Init_Timer1(void) //10毫秒@12MHz,100Hz刷新频率 { 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_L,PWM_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--; } //右电机后退 }