STM32簡單的PID控制器代碼

/**************************************************************************
函數功能：位置式PID控制器
入口參數：編碼器測量位置信息，目標位置
返回  值：電機PWM
根據位置式離散PID公式 
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  
∑e(k)代表e(k)以及之前的偏差的累積和;其中k爲1,2,,k;
pwm代表輸出
**************************************************************************/
int Position_PID (int Encoder,int Target)
{ 	
	 float Position_KP=80,Position_KI=0.1,Position_KD=500;
	 static float Bias,Pwm,Integral_bias,Last_Bias;
	 Bias=Encoder-Target;                                  //計算偏差
	 Integral_bias+=Bias;	                                 //求出偏差的積分
	 Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
	 Last_Bias=Bias;                                       //保存上一次偏差 
	 return Pwm;                                           //增量輸出
}

/**************************************************************************
函數功能：增量PI控制器
入口參數：編碼器測量值，目標速度
返回  值：電機PWM
根據增量式離散PID公式 
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差 
e(k-1)代表上一次的偏差  以此類推 
pwm代表增量輸出
在我們的速度控制閉環系統裏面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI (int Encoder,int Target)
{ 	
   float Kp=20,Ki=30;	
	 static int Bias,Pwm,Last_bias;
	 Bias=Encoder-Target;                //計算偏差
	 Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //增量式PI控制器
	 Last_bias=Bias;	                   //保存上一次偏差 
	 return Pwm;                         //增量輸出
}