K60正交解码测速，PID调节不知哪里的问题

空格_Space

程序 如下：

/*作用：改变电机占空比
**形参：0~10000，对应占空比为0%~100%
**返回值：无
*/
int16 PWM;
int16 PWM_Out=0;
int16 S_PWM_MAX=3000,S_PWM_MIN=0;
int16 S_Error=0,S_Lasterror=0,S_Preerror=0;
float S_Kp=5.4,S_Ki=0.00,S_Kd=0;  //pid 的值待定
void dianji_dutycycle_fore()
{
  int16 count;
  /*
  PWM=3000-8*(C_div*C_div);
  OLED_ShowNum(80,40,PWM,4,16);
  count=C_div*C_div;
  if(count>=300)  PWM=2000;  //17*17=300;
  if(PWM<=2000)  PWM=2000;
  */
  PWM=2500;
  printf("\rpwm_EXPECT=%d\n",PWM);
  S_Preerror=S_Lasterror;
  S_Lasterror=S_Error;
  S_Error=PWM-(qd_result);
  printf("\rqd=%d\n",qd_result);
  printf("\rS_Error=%d\n",S_Error);
  printf("\rS_Last=%d\n",S_Lasterror);
  printf("\rDiv_E_L=%d\n",S_Error-S_Lasterror);
  //OLED_ShowNum(40,0,S_Error,4,16);
  PWM_Out+=S_Kp*(S_Error-S_Lasterror)+S_Ki*(S_Error)+\
           S_Kd*(S_Error-2*S_Lasterror+S_Preerror) ;
  if(PWM_Out>=S_PWM_MAX)
          PWM_Out=S_PWM_MAX;
  if(PWM_Out<=S_PWM_MIN)
          PWM_Out=S_PWM_MIN;
  OLED_ShowNum(35,40,PWM,4,16);
  OLED_ShowNum(0,0,S_Error,5,16);
  
  printf("\rpwm_REAL=%d\n",PWM_Out);
  OLED_Refresh_Gram();   
  LPLD_FTM_PWM_ChangeDuty(FTM1,FTM_Ch0,PWM_Out);  //改变电机占空比
  LPLD_FTM_PWM_ChangeDuty(FTM1,FTM_Ch1,0);  
}

这个的现象就是加速药很长时间，同时，到了速度之后，也不能稳定在制定值，波动可以说非常大，
大家看看这个程序有没问题？？？？