舵机PD程序

GZFSJK

     /*************计算偏差********************/
if(subvalue==0)
  {
    controlvalue=0;
  }
  else
  {
    controlvalue = (subvalue/addvalue)*10;//偏差
  }
  
  Turn1_Err = controlvalue;//偏差
  Turn_EC = Turn1_Err - Turn2_Err ;           //  计算新的偏差变化值

  Turn_P_Value =  Turn1_Err * turn14_KP;       //  增量式PID控制计算P调节量
  Turn_D_Value =  Turn_EC   *  turn14_KD;     //  增量式PID控制计算D调节量
  
  TurnPWM14 = -(int16_t)( Turn_P_Value  + Turn_D_Value );
  Turn2_Err = Turn1_Err;//上次偏差
   
  WirelessSerial (Turn2_Err*100,Turn1_Err*100,Turn_EC*100,TurnPWM14);  
  if(TurnPWM14>=1000) TurnPWM14 =1000 ;
  if(TurnPWM14 <= -1000) TurnPWM14 = -1000;   
   Apwm14 = 4530 + TurnPWM14;         //没有左转和右转是舵机回到中位。150hz时中间为2200左右范围1000
   
   return (int)Apwm14 ;   //将Apwm值返回
   

GZFSJK

为啥，感觉不对啊？