一、簡介

續上文，電機驅動、MPU6050驅動均已完成，接下來我們給他加上PID讓它站起來

二、PID控制

我們將小車PID控制函數放到MPU6050中斷函數中，每當MPU6050有數據輸出時，引腳INT有相應的電平輸出。依次來觸發外部中斷作為控制周期。保持MPU6050數據的實時性。

2.1直立環控制

通過傳入的KP、KD參數，讓平衡車的pitch軸保持在水平位置。

直立環是用于控制機械系統中垂直方向運動的一種控制器。在這些應用中，垂直方向的運動通常是非常穩定的，因此I控制器的積分作用并不是必需的。而PD控制器可以提供足夠的控制性能，同時避免了可能出現的積分飽和問題，因此在直立環中通常只使用PD控制器，而不使用I控制器。

/**************************************************************************

函數功能：直立PD控制

入口參數：角度、機械平衡角度（機械中值）、角速度

返回 值：直立控制PWM

**************************************************************************/

int balance_UP(float Angle,float Mechanical_balance,float Gyro)

{ 

  float Bias;

  int balance;

  Bias=Angle-Mechanical_balance;   //求出平衡的角度中值和機械相關

  balance=balance_UP_KP*Bias+balance_UP_KD*Gyro; //計算平衡控制的電機PWM PD控制  kp是P系數 kd是D系數

  return balance;

}

2.2速度環控制

這里我們只用到了PI的控制參數，如果有微分環(D)在里面的，系統會響應波動太快，微分對極小的速度波動都會有反應，而速度的微微波動又太多了，什么電機微抖，轉速檢測裝置的不穩定。總結，微分太靈敏了，所以我們只用到了PI作為調節參數

/**************************************************************************

函數功能：速度PI控制

入口參數：電機編碼器的值

返回 值：速度控制PWM

**************************************************************************/

int velocity(int encoder_left,int encoder_right,int Target_Speed)

{ 

   static float Velocity,Encoder_Least,Encoder;

  static float Encoder_Integral;

  //=============速度PI控制器=======================// 

  Encoder_Least=(Encoder_Left+Encoder_Right); //獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度

  Encoder *= 0.8; //===一階低通濾波器      

  Encoder += Encoder_Least*0.2; //===一階低通濾波器  

  Encoder_Integral +=Encoder; //===積分出位移 積分時間：10ms

  Encoder_Integral=Encoder_Integral-Target_Speed;  //接收遙控器數據，控制前進后退

  if(Encoder_Integral>10000)   Encoder_Integral=10000; //===積分限幅

  if(Encoder_Integral<-10000)?????? Encoder_Integral=-10000; //===積分限幅?????? 

 ????? Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI; //===速度控制???? 

   ?if(pitch<-40||pitch>40)   Encoder_Integral=0;   //===電機關閉后清除積分

  return Velocity;

}

2.3轉向環控制

在轉向控制中，PD控制器代表的是比例-微分控制器，它根據當前的偏差和偏差的變化率來調整控制參數，以實現快速響應和穩定性。

相比之下，積分控制器（I控制器）會根據偏差的積分來調整控制參數，以減小系統的穩態誤差。然而，在轉向控制中，由于小車的動態響應和環境的不確定性，I控制器容易出現積分飽和的問題，從而導致系統不穩定和震蕩。

因此，為了保證轉向控制的穩定性和可靠性，通常只使用PD控制器，而不使用I控制器。同時，還可以通過其他控制策略，如模糊控制、自適應控制等來進一步提高控制性能。

/**************************************************************************

函數功能：轉向PD控制

入口參數：電機編碼器的值、Z軸角速度

返回 值：轉向控制PWM

**************************************************************************/



int Turn_UP(int gyro_Z, int RC)

{

  int PWM_out;

  /*轉向約束*/

  if(RC==0)Turn_Kd=TURN_KD;//若無左右轉向指令，則開啟轉向約束

  else Turn_Kd=0;//若左右轉向指令接收到，則去掉轉向約束

  PWM_out=Turn_Kd*gyro_Z + Turn_KP*RC;

  return PWM_out;

}

2.4 PID參數的調節

通過調節PID的參數，使小車達到穩定狀態，該數據僅供參考，具體調節還得從實際中得來

//直立環

#define BLC_KP 300

#define BLC_KD 0.6

//速度環

#define SPD_KP -80

#define SPD_KI -0.40

//轉向環

#define TURN_KP -30

#define TURN_KD -0.3