|
|
/**************************************************************************
函數功能：位置式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代表輸出
**************************************************************************/
int16_t PID_Ctrl (int16_t Target_val,int16_t Actual_val )
{
Bias=Target_val-Actual_val; // 計算偏差
Integral_bias+=Bias; // 求出偏差的積分
Pwm1 =Kp*Bias; //
Pwm2 =Ki*Integral_bias; //
if (Pwm2>=5600) Pwm2=5600;
else if(Pwm<=-5600) Pwm2=-5600;
// Pwm3 =Kd*(Bias-Last_Bias); // 位置式PID控制器
Pwm =Kdim*(Pwm1+Pwm2) +1200;
// Pwm =Kdim*(Pwm1+Pwm2) +0x11a0;
Last_Bias=Bias; // 保存上一次偏差
return Pwm; // 增量輸出
}
|
|
