智能车制作

标题: 电磁 [打印本页]

作者: GZFSJK 时间: 2017-5-21 13:28
标题: 电磁
电磁圆环过的时候左右摇摆一不小心就出去了，怎么办？程序上就用了一个小的判断
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值返回

}

欢迎光临智能车制作 (http://dns.znczz.com/)