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, 下載次數: 183)
2019-7-21 15:54 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
|
[復制鏈接]
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
