代碼

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

調參

直立環
屏蔽速度環&轉向環

1.機械中值
小車本身盡可能不倒下的Pitch值。

2.Kp極性（正負）
極性正確：小車呈直立趨勢。
極性錯誤：小車加速倒下。

3.Kp大小
數值從小到大嘗試，直到小車出現大幅低頻抖動。

4.Kd極性（正負）
屏蔽直立環的Kp
極性正確：小車輪胎與小車轉動方向相同。
極性錯誤：小車輪胎與小車轉動方向相反。

5.Kd大小
加入直立環Kp
數值從小到大嘗試，直到小車出現高頻振蕩。

Kp與Kd最終數值=0.6?振蕩數值。
速度環
Ki=1/200Kp*
1.Kp極性
屏蔽直立環&轉向環
極性正確：旋轉小車其中一個輪子，兩輪同向轉動，直至轉速最大。
極性錯誤：旋轉小車其中一個輪子，另一輪子反向轉動。

2.Kp大小
加入直立環
數值從小到大嘗試，直到小車走直線性能優秀，回位效果良好。

轉向環
屏蔽直立環&速度環

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。