舵机控制

zhuzhuyanrong · 发表于 2012-5-4 16:10:16

求舵机控制策略？

hsx1612727380 · 发表于 2012-5-4 16:10:17

/*
******************************
***舵机控制位置式PID
******************************
***入口参数Line_Center[hang]
******************************
*/
void Sever_turn_control(void)  //舵机位置式PID
{
;
}
int currentPWM=0,prePWM;
void public_turn_control(char _center)
{
history=PWMDTY01;
PWMDTY01=((_center)/2)*20+3050;
if((PWMDTY01>history+500) || (PWMDTY01<history-500))
{
      PWMDTY01=history;
      PORTB_PB1=1;
}
else
{
      PORTB_PB1=0;
}
if(chang_count%2==1)
{
      prePWM=PWMDTY01;
}
else if(chang_count%2==0)
{
      if(PWMDTY01>prePWM+30 || PWMDTY01<prePWM-30)
      {
         PWMDTY01=prePWM;
         PORTB_PB1=1;
      }
      else
      {
         PORTB_PB1=0;
      }

} /**/
switch(_center/2)                //MID 47*8.5=400
{
   case 0:  PWMDTY01=3120;break; //
   case 1:  PWMDTY01=3140;break;
   case 2:  PWMDTY01=3157;break;
   case 3:  PWMDTY01=3174;break;
   case 4:  PWMDTY01=3291;break;  //
   case 5:  PWMDTY01=3209;break;
   case 6:  PWMDTY01=3226;break;
   case 7:  PWMDTY01=3243;break;
   case 8:  PWMDTY01=3260;break;//
   case 9:  PWMDTY01=3278;break;
   case 10: PWMDTY01=3296;break;
   case 11: PWMDTY01=3313;break;
   case 12: PWMDTY01=3330;break;//
   case 13: PWMDTY01=3348;break;
   case 14: PWMDTY01=3365;break;
   case 15: PWMDTY01=3382;break;
   case 16: PWMDTY01=3399;break;//
   case 17: PWMDTY01=3417;break;
   case 18: PWMDTY01=3434;break;
   case 19: PWMDTY01=3451;break;
   case 20: PWMDTY01=3468;break;//3ge
   case 21: PWMDTY01=3486;break;
   case 22: PWMDTY01=3503;break;
   case 23: PWMDTY01=3520;break;
   case 24: PWMDTY01=3520;break;  //3536
   case 25: PWMDTY01=3553;break;
   case 26: PWMDTY01=3569;break;//
   case 27: PWMDTY01=3586;break;
   case 28: PWMDTY01=3603;break;  //
   case 29: PWMDTY01=3619;break;
   case 30: PWMDTY01=3636;break;  //
   case 31: PWMDTY01=3652;break;
   case 32: PWMDTY01=3670;break; //
   case 33: PWMDTY01=3686;break;
   case 34: PWMDTY01=3703;break; //
   case 35: PWMDTY01=3720;break;
   case 36: PWMDTY01=3737;break; //
   case 37: PWMDTY01=3753;break;
   case 38: PWMDTY01=3770;break; //
   case 39: PWMDTY01=3786;break;
   case 40: PWMDTY01=3804;break; //
   case 41: PWMDTY01=3820;break;
   case 42: PWMDTY01=3837;break;  //
   case 43: PWMDTY01=3853;break;
   case 44: PWMDTY01=3870;break;  //
   case 45: PWMDTY01=3886;break;
   case 46: PWMDTY01=3903;break;  //
   case 47: PWMDTY01=3920;break;
} //参数自己调啊