代碼

#include "control.h"float Med_Angle = -1.4; //機(jī)械中值（需調(diào)） float Vertical_Kp = -348, //直立環(huán)Kp（需調(diào)） Vertical_Kd = -0.9; //直立環(huán)Kd（需調(diào)） float Velocity_Kp = 0.35, //速度環(huán)Kp（需調(diào)）Velocity_Ki = 0.00175; //速度環(huán)Ki（需調(diào)）float Turn_Kp=1; //轉(zhuǎn)向環(huán)Kp（需調(diào)）int Vertical_out,Velocity_out,Turn_out; //直立環(huán)&速度環(huán)&轉(zhuǎn)向環(huán)輸出變量void EXTI9_5_IRQHandler(void) {if(EXTI_GetITStatus(EXTI_Line5)!=0) //一級(jí)判定{int PWM_out;if(PBin(5)==0) //二級(jí)判定{EXTI_ClearITPendingBit(EXTI_Line5); //清除中斷標(biāo)志位/*步驟1.采集編碼器數(shù)據(jù)&MPU6050角度信息*/Encoder_Left = -Read_Speed(2);Encoder_Right = Read_Speed(4); /**********************正點(diǎn)原子例程**********************/mpu_dmp_get_data(&Pitch,&Roll,&Yaw); MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);MPU_Get_Accelerometer(&aacx,&aacy,&aacz);//2.將數(shù)據(jù)帶入直立環(huán)&速度環(huán)&轉(zhuǎn)向環(huán)，計(jì)算出控制輸出量Vertical_out = Vertical(Med_Angle,Pitch,gyroy);Velocity_out = Velocity(Encoder_Left,Encoder_Right);Turn_out = Turn(gyroz);PWM_out = Vertical_out - Vertical_Kp*Velocity_out; /*****************************************************/ //3.將控制輸出量加載至電機(jī)MOTO1 = PWM_out-Turn_out; //電機(jī)1MOTO2 = PWM_out+Turn_out; //電機(jī)2Limit(&MOTO1,&MOTO2); //PWM限幅Load(MOTO1,MOTO2); //加載至電機(jī)}} }/**************** 直立環(huán)PD:Kp*Ex+Kd*Ex_D 入口參數(shù)：Med真實(shí)角度，Angle期望角度，gyro_Y真實(shí)角速度 出口參數(shù)：直立環(huán)輸出 ****************/ int Vertical(float Med,float Angle,float gyro_Y) {int PWM_out;PWM_out = Vertical_Kp*(Angle-Med) + Vertical_Kd*(gyro_Y-0);return PWM_out; }/**************** 速度環(huán)PI:Kp*Ek+Ki*Ek_s ****************/ int Velocity(int encoder_left,int encoder_right) {static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_last;float a = 0.7;//1.計(jì)算速度偏差Encoder_Err = (encoder_left+encoder_right)-0; //這里左右電機(jī)之和沒(méi)有除以2是為了避免舍去誤差//2.對(duì)速度誤差進(jìn)行低通濾波//low_out=(1-a)*Ek+a*low_out_last;EnC_Err_Lowout = (1-a)*Encoder_Err + a*EnC_Err_Lowout_last;EnC_Err_Lowout_last = EnC_Err_Lowout;//3.對(duì)速度誤差積分，積分出位移Encoder_S += EnC_Err_Lowout;//4.積分限幅Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);//5.速度環(huán)控制積分計(jì)算PWM_out = Velocity_Kp*EnC_Err_Lowout + Velocity_Ki*Encoder_S;return PWM_out; }/**************** 轉(zhuǎn)向環(huán)：Kp*gyro_Z ****************/ int Turn(int gyro_Z) {int PWM_out;PWM_out = Turn_Kp*gyro_Z;return PWM_out; }

調(diào)參

直立環(huán)
屏蔽速度環(huán)&轉(zhuǎn)向環(huán)

1.機(jī)械中值
小車(chē)本身盡可能不倒下的Pitch值。

2.Kp極性（正負(fù)）
極性正確：小車(chē)呈直立趨勢(shì)。
極性錯(cuò)誤：小車(chē)加速倒下。

3.Kp大小
數(shù)值從小到大嘗試，直到小車(chē)出現(xiàn)大幅低頻抖動(dòng)。

4.Kd極性（正負(fù)）
屏蔽直立環(huán)的Kp
極性正確：小車(chē)輪胎與小車(chē)轉(zhuǎn)動(dòng)方向相同。
極性錯(cuò)誤：小車(chē)輪胎與小車(chē)轉(zhuǎn)動(dòng)方向相反。

5.Kd大小
加入直立環(huán)Kp
數(shù)值從小到大嘗試，直到小車(chē)出現(xiàn)高頻振蕩。

Kp與Kd最終數(shù)值=0.6?振蕩數(shù)值。
速度環(huán)
Ki=1/200Kp*
1.Kp極性
屏蔽直立環(huán)&轉(zhuǎn)向環(huán)
極性正確：旋轉(zhuǎn)小車(chē)其中一個(gè)輪子，兩輪同向轉(zhuǎn)動(dòng)，直至轉(zhuǎn)速最大。
極性錯(cuò)誤：旋轉(zhuǎn)小車(chē)其中一個(gè)輪子，另一輪子反向轉(zhuǎn)動(dòng)。

2.Kp大小
加入直立環(huán)
數(shù)值從小到大嘗試，直到小車(chē)走直線性能優(yōu)秀，回位效果良好。

轉(zhuǎn)向環(huán)
屏蔽直立環(huán)&速度環(huán)

總結(jié)

以上是生活随笔為你收集整理的STM32两轮自平衡小车——PID的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。