STM32F103C8T6，跷跷板平衡车，MPU6050，PID
元件清单:
OLED显示屏
STM32F103C8T6最小系统
MPU6050陀螺仪
TB6612FNG驱动模块
LM2596稳压
12V锂电池
直流减速电机带编码器*2
红外循迹探头*5
37mm直流减速电机固定座 铁质*2
65mm轮胎*2
铁质 连轴器*2
杜邦线若干
万向轮
铜柱
洞洞板
单片机源程序如下:
- #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;
- }
- }
- }
- }
- }
- }
- }
所有资料51hei提供下载:
project+注释.7z
(68.97 KB, 下载次数: 179)
|
[复制链接]
