STM32F103C8T6单片机跷跷板平衡车程序，MPU6050，PID -

#include "stm32f10x.h"
#include "sys.h"
char x[10];//角度显示缓冲数组
u8 Tim[7]= "Time : ";//时间显示数组
extern float Roll; //读取到的6050角度值
float Roll_1,Roll_2,Roll_3;//分别是最新时刻角度值，上一时刻角度值，以及积分角度值
float power;//小车最终输出动力
float KP=8.0,KD=0.5,KI=0.02,PrevError_C =0;//PID参数
u8 pid_flag=0;//分时刻读取角度值的标志位
extern u16 tim_flag;//计时标志位
extern int tim;//秒计时，一秒加一
double zz=0;//平衡计时
int main()//主函数
{
delay_init();//延时函数初始化
IIC_Init();//IIC初始化
MPU6050_initialize();//6050初始化
DMP_Init();//MPU6050内置DMP的初始化
OLED_Init();//OLED初始化
OLED_Clear();//OLED清屏
GPIO_INIT();//控制减速电机IO口初始化
HW_INIT();//红外探头使用IO口初始化
TIM3_INIT();//定时器初始化，用于放PWM波，控制车的速度
Qianjin();//使车向前跑（相对的）
TIM3->CCR1=80;//左轮动力80
TIM3->CCR2=80;//右轮动力80
OLED_ShowString(0,4, Tim);//OLED显示不改变数组
Read_DMP();//读取一次角度值
while(1)//无限循环
{
sprintf(x,"Angle: %0.2f",Roll);//此函数用于将数组加入可变参数
OLED_ShowString(0,0,(u8*)x);//显示上列数组
if(tim>=99)tim=99;//计时上限99秒
OLED_ShowNum(48,4,tim,3,12);//显示时间，单位/秒
OLED_ShowNum(0,6,zz,4,12);//显示平衡时间
OLED_ShowNum(32,6,power,4,12);//显示当前动力值
/***************************平衡PID算法********************************/
pid_flag++;//运行时间标志
if(pid_flag==1)//记录1时刻角度值
{
Roll_1=Roll;
}
if(pid_flag==2)//记录2时刻角度值
{
pid_flag=0;
Roll_2=Roll;
PrevError_C=Roll_2-Roll_1;//两次角度的差值
}
Read_DMP();//读取当前角度值
Roll_3+=Roll;//积分值累加
power=KP*abs(Roll)+KI*abs(Roll_3)+KD*PrevError_C;//PID计算当前合适的动力值
if(power>90) power=90;//动力值限幅
if(Roll>1.7)//判断角度值
{
if(PrevError_C<-0.03)//预判是否需要后退
Qianjin();
else //不需要就继续走
Houtui();
TIM3->CCR1=power+30;
TIM3->CCR2=power;
}
if(Roll<-1.7) //判断角度值
{
if(PrevError_C>0.03)//预判是否需要后退
Houtui();
else //不需要就继续走
Qianjin();
TIM3->CCR1=power+20;
TIM3->CCR2=power;
}
if((Roll<=1.7)&&(Roll>=-1.7)&&(PrevError_C<0.03&&PrevError_C>-0.03))//平衡条件限定
{
Tingzhi();//停止
TIM3->CCR1=0;//动力值为0
TIM3->CCR2=0;
Roll_3=0; //积分清零
zz++;//平衡计时
if(zz>888)//达到平衡时间观察
{
while(1)
{
Qianjin();TIM3->CCR1=95;TIM3->CCR2=80;//匀速向前
if(HW5==0)//右后探头检测到黑线
{
Tingzhi();//停止
while(1)
{
Tingzhi();
TIM3->CCR1=0;//动力为0
TIM3->CCR2=0;
}
}
}
}
}
}
}

复制代码