舵机打脚问题，用多少行的偏差，该怎么处理偏差,求平均?

笨小孩有梦想

yyuiopxx 发表于 2017-4-11 23:16
先确认一下舵机的硬件供应和配置是否正确，再来排除算法的问题。

大神请问一下我这个部分程序可有什么问题么
    //IO管脚配置
    gpio_init(MOTOR1_IO,GPO,LOW);
    gpio_init(MOTOR2_IO,GPO,LOW);
    gpio_init(MOTOR3_IO,GPO,LOW);
    gpio_init(MOTOR4_IO,GPO,LOW);
a=0;b=0;c=0;d=0;


    while(1)
    {

       ftm_quad_init(FTM2);                                    //FTM1 正交解码初始化                                                           //所用的管脚可查 port_cfg.h 的 FTM1_QDPHA_PIN 和 FTM1_QDPHB_PI
    pit_init_ms(PIT0, 100);                                 //初始化PIT0，定时时间为： 500ms
    set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler);      //设置PIT0的中断服务函数为 PIT0_IRQHandler
    enable_irq (PIT0_IRQn);                                 //使能PIT0中断
      for(K=0;K<5;K++)
{
         A[K]= adc_once   (AMP1, ADC_8bit);
         B[K]= adc_once   (AMP2, ADC_8bit);
         C[K]= adc_once   (AMP3, ADC_8bit);
         D[K]= adc_once   (AMP4, ADC_8bit);
}
        for(K=0;K<5;K++)
{
        a=a+A[K];
        b=b+B[K];
        c=c+C[K];
        d=d+D[K];
}
        e=(a+c)/60;
      f=(b+d)/60;
        h=e-f;
        j=f-e;
        i=150-h;

        if(h>=12)
{
          g=h*8;
}
        else if(j>=10)
        {
          g=j*8;
        }
            ftm_pwm_duty(MOTOR_FTM, MOTOR1_PWM,0);
        ftm_pwm_duty(MOTOR_FTM, MOTOR2_PWM,400-g);

         if((a+b+c+d)<=1200)
           {

            if((a+c)>=(b+d))
            {
             ftm_pwm_duty(S3010_FTM, S3010_CH,i-30);
             DELAY_MS(100);
           }
          if((a+c)<(b+d))
            {
             ftm_pwm_duty(S3010_FTM, S3010_CH,i+30);
             DELAY_MS(100);
           }
           }
            ftm_pwm_duty(S3010_FTM, S3010_CH,i);


     g=0;
  a=b=c=d=0;
    }

    }