位置式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代表输出

**************************************************************************/
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;                             // 增量输出
}