|
|
/**************************************************************************
函数功能：位置式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; // 增量输出
}
|
|
