电磁

GZFSJK

电磁圆环过的时候左右摇摆一不小心就出去了，怎么办？程序上就用了一个小的判断
int16 Turn14(float LeftAD,float RightAD,float LAD,float RAD)
{
  float TurnPWM14, Apwm14;


  static float Turn1_Err;
  
  float Turn2_Err,Turn_EC;
  float subvalue1,subvalue2, addvalue,controlvalue;
  float Turn_P_Value,Turn_D_Value;


  if(LeftAD<40) LeftAD=0;
  if(RightAD<40) RightAD=0;


  subvalue1 = LeftAD-RightAD;
  subvalue2 =LAD-RAD;
  addvalue = LeftAD+RightAD+LAD+RAD;
     /*************计算偏差********************/
if(subvalue1==0)
  {
    controlvalue=0;
  }
  else
  {
    controlvalue = ((subvalue1)/addvalue)*10;//偏差
  }
  Turn2_Err = Turn1_Err;//上次偏差
  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 );


//     WirelessSerial (TurnPWM14,Turn_P_Value,addvalue,Turn_D_Value);

  if(TurnPWM14>=800) TurnPWM14 =800 ;
  if(TurnPWM14 <= -800) TurnPWM14 = -800;   
   Apwm14 = 4530 + TurnPWM14;         //没有左转和右转是舵机回到中位。150hz时中间为2200左右范围1000
        /****************圆环*******************/
      if((LeftAD<450)&&(RightAD<450))
      {
        return 3800;   //将Apwm值返回
      }
    else
   return (int)Apwm14 ;   //将Apwm值返回
   
}