Exemplo n.º 1
0
/*******************************************************************************
* Function Name  : motor_test()
* Description    : 电机驱动测试
* Input          : None
* Output         : None
* Return         : None
* Editor         :lyf
***************************************************************************/
void SERVO_test()
{

  ftm_pwm_duty(FTM3, SERVO, 1100);  //R
  DELAY_MS(1000);


  ftm_pwm_duty(FTM3, SERVO, 900);
  DELAY_MS(1000);

}
Exemplo n.º 2
0
/*******************************************************************************
* Function Name  : MotorSpeedOut(float standPWM ,float speedPWM ,float turnPWM)
* Description    : 电机驱动
* Input          : None
* Output         : None
* Return         : None
* Editor         :yzy
***************************************************************************/
void MotorSpeedOut(float Speed_L, float Speed_R)
{

  /***********************将最大速度限制在985个PWM内******************************/
  if (Speed_L > 10000)  Speed_L = 10000;
  if (Speed_L < -10000) Speed_L = -10000;
  if (Speed_R > 10000)  Speed_R = 10000;
  if (Speed_R < -10000) Speed_R = -10000;

  /*************用所得到的对应角度的速度进行PWM控制********************/
  if (Speed_L >= 0)   //angle大于0,向前,小于0,向后
  {
    ftm_pwm_duty(FTM0, MOTOR1_PWM, 0);
    ftm_pwm_duty(FTM0, MOTOR2_PWM, (uint32)(Speed_L - MOTOR_DEAD_VAL_L));  //加入死区电压
  }
  else
  {
    ftm_pwm_duty(FTM0, MOTOR2_PWM, 0);
    ftm_pwm_duty(FTM0, MOTOR1_PWM, (uint32)(-Speed_L - MOTOR_DEAD_VAL_L));  //加入死区电压
  }

  if (Speed_R >= 0)   //angle大于0,向前,小于0,向后
  {
    ftm_pwm_duty(FTM0, MOTOR4_PWM, 0);
    ftm_pwm_duty(FTM0, MOTOR3_PWM, (uint32)(Speed_R - MOTOR_DEAD_VAL_R));  //加入死区电压
  }
  else
  {
    ftm_pwm_duty(FTM0, MOTOR3_PWM, 0);
    ftm_pwm_duty(FTM0, MOTOR4_PWM, (uint32)(-Speed_R - MOTOR_DEAD_VAL_R)); //加入死区电压
  }
}
Exemplo n.º 3
0
/*******************************************************************************
* Function Name  : motor_test()
* Description    : 电机驱动测试
* Input          : None
* Output         : None
* Return         : None
* Editor         :lyf
***************************************************************************/
void motor_test(uint32 du)
{

  ftm_pwm_duty(MOTOR_FTM, MOTOR1_PWM, 0);
  ftm_pwm_duty(MOTOR_FTM, MOTOR2_PWM, du); //L    给占空比正转
  ftm_pwm_duty(MOTOR_FTM, MOTOR3_PWM, du); //R    给占空比正转
  ftm_pwm_duty(MOTOR_FTM, MOTOR4_PWM, 0);
//      DELAY_MS(1000);


  //ftm_pwm_duty(MOTOR_FTM, MOTOR1_PWM,0);
  //ftm_pwm_duty(MOTOR_FTM, MOTOR2_PWM,2000);
  //ftm_pwm_duty(MOTOR_FTM, MOTOR3_PWM,0);
  //ftm_pwm_duty(MOTOR_FTM, MOTOR4_PWM,2000);
  //DELAY_MS(1000);


}
Exemplo n.º 4
0
void SD5_control(void)
{
       int i;  //循环用
       int sum=0;
        float error_average; 
        float SD_center =1000-medium_duty ;//居中                
	float pwm_SD5;
        float error_last=0;
        
        d=d_PID;
	for(i=img_upper;i<img_down;i++)                  //视野范围
	{
		sum=sum+centerline[i];
	}
	error_average=sum/(img_down-img_upper);                            
        error_last=error_now;   //当前偏差变为上一次偏差
	error_now=CAMERA_W/2-error_average;  //计算当前偏差
        p = p_wandao;
        d = d_PID;
        lianxu_S();
        pwm_SD5 = SD_center + p*error_now+d*(error_now-error_last);      // 实际传入舵机的占空比为duty_medium-p*error;
        
 
//限幅        
        if(pwm_SD5<1000-right_duty) pwm_SD5= 1000 - right_duty;                 
        if(pwm_SD5>1000-left_duty) pwm_SD5= 1000 - left_duty;
//过弯打死
        if(error_now>40)
          pwm_SD5 = 1000-left_duty;
        if(error_now<-40)
          pwm_SD5 = 1000-right_duty;
//转弯降速           
        if((uint32)pwm_SD5<(1000-360)||(uint32)pwm_SD5>(1000-540))
        { 
          ftm_pwm_duty(MOTOR_FTM, MOTOR1_PWM,motor_wandao);
          ftm_pwm_duty(MOTOR_FTM, MOTOR2_PWM,motor);
        }
                 
          ftm_pwm_duty(SD5_FTM,SD5_CH,(uint32)pwm_SD5);
        
        
}
Exemplo n.º 5
0
void speed_control(void)
{    
         
         motor.speed_last_error = motor.speed_current_error ;        //更新每次的差值
         motor.speed_current_error = motor.speed_set-motor.speed;   //速度当前差值

         motor.speed_duty_output = motor.speed_duty_output+(int16)(motor.speed_p*(motor.speed_current_error-motor.speed_last_error)+motor.speed_i*motor.speed_current_error);      

     
        if(motor.speed_duty_output>=900)
            motor.speed_duty_output = 900;
        if(motor.speed_duty_output<=-900)
              motor.speed_duty_output = -900; 
        
        if(motor.speed < 25 )
        {
            ftm_pwm_duty(GO_FTM,GO_CH,0);
            ftm_pwm_duty(BACK_FTM,BACK_CH,0); 
        }
        else
        {  
            if(motor.speed_duty_output>=0)    
            {
               ftm_pwm_duty(GO_FTM,GO_CH,motor.speed_duty_output);
               ftm_pwm_duty(BACK_FTM,BACK_CH,0);      
            }
            else
            {        
                ftm_pwm_duty(GO_FTM,GO_CH,0);
                ftm_pwm_duty(BACK_FTM,BACK_CH,-motor.speed_duty_output);          
            }   
        }
}
Exemplo n.º 6
0
/************电机输出函数*************/
void SetMotorVoltage(float fLeftVoltage,float fRightVoltage)
{
    int nOut;
    if(fLeftVoltage>0)
    {
        ftm_pwm_duty(FTM0,FTM_CH3,0);//左轮正向运动PWM占空比为0
        nOut=(int)(fLeftVoltage*PERIOD);//
        if(nOut>1000)
        {
            nOut=1000;
        }
        ftm_pwm_duty(FTM0,FTM_CH4,nOut);//左轮反向运动PWM占空比为nOut
    }                                                   //左电机正转
    else
    {
        ftm_pwm_duty(FTM0,FTM_CH4,0);//左轮反向运动PWM占空比为0
        fLeftVoltage=-fLeftVoltage;
        nOut=(int)(fLeftVoltage*PERIOD);
        if(nOut>1000)
        {
            nOut=1000;
        }
        ftm_pwm_duty(FTM0,FTM_CH3,nOut);//左轮正向运动PWM占空比为nOut
    }                                                    //左电机反转

    if(fRightVoltage>0)
    {
        ftm_pwm_duty(FTM2,FTM_CH0,0);//右轮正向运动PWM占空比为0
        nOut=(int)(fRightVoltage*PERIOD);
        if(nOut>1000)
        {
            nOut=1000;
        }
        ftm_pwm_duty(FTM2,FTM_CH1,nOut);//右轮反向运动PWM占空比为nOut
    }                                                     //右电机正转
    else
    {
        ftm_pwm_duty(FTM2,FTM_CH1,0);//右轮反向运动PWM占空比为0
        fRightVoltage=-fRightVoltage;
        nOut=(int)(fRightVoltage*PERIOD);
        if(nOut>1000)
        {
            nOut=1000;
        }
        ftm_pwm_duty(FTM2,FTM_CH0,nOut);//右轮正向运动PWM占空比为nOut
    }                                                     //右电机反转
}
Exemplo n.º 7
0
void set_servo(servo_path path,duty_t angle)
{
    duty_t pwm_out=0;
    
    if(path==servo_right)
    {
        if(angle>1100)
            pwm_out=FTM1_PRECISON-(median-1100);
        else
            pwm_out=FTM1_PRECISON-(median-angle);
    }
    else if(path==servo_left)
    {
        if(angle>1150)
            pwm_out=FTM1_PRECISON-(median+1150);
        else
            pwm_out=FTM1_PRECISON-(median+angle);
    }
    
    ftm_pwm_duty(servo_FTM, servo_CH,pwm_out);
}
Exemplo n.º 8
0
/********************************舵机PD控制*************************************/
void PDkongzhi()
{

    P = actuator_P, D = actuator_D;

    LastError = Error; //上一次偏差
    // if(left_diuxian1 > 10 && right_diuxian1 < 10 &&
    //  shuang_diuxian1 < 10 && slope.right > -0.01 &&
    //   slope.right < 0.01 && slope.right!=0)
    // {
    //        Error = 15;
    //        led(LED1,LED_ON);
    // }
    // else if(right_diuxian1 > 10 && left_diuxian1 < 10 &&
    //         shuang_diuxian1 < 10 && slope.left > -0.01 &&
    //         slope.left < 0.01 && slope.left!=0)
    // {
    //        Error = -15;
    //        led(LED1,LED_ON);
    // }
    if(left_diuxian1 > 10 && right_diuxian1 < 10 && wide_check())
    {
           Error = 0;
           led(LED1,LED_ON);
    }
    else if(right_diuxian1 > 10 && left_diuxian1 < 10 && wide_check())
    {
           Error = 0;
           led(LED1,LED_ON);
    }
    else
    {
       Error = black_centre  + (int)(slope.slope * 160); //平均中点和理论中点0之间的偏差 + 斜率
       led(LED1,LED_OFF);
    }

    dajiao = control_actuator_center - (int)(P * (float)Error) - (int)(D * (float)(Error - LastError));//舵机打角

    if(dajiao > control_actuator_max) //左最大
        dajiao = control_actuator_max; 
    else if(dajiao < control_actuator_min) //右最大
        dajiao = control_actuator_min;

    if(Error>35)
        Error=35;
    if(Error<-35)
        Error=-35;


    ftm_pwm_duty(S3010_FTM, S3010_CH, (uint32)dajiao);
    //SetMotorVoltage(speedl , speedr );

    if(stop_done == 1)
    {
        // #if ( CAR_NUMBER == 2 )
        // lptmr_delay_ms(1000);    //延时防撞
        // #endif
        SetMotorVoltage(0, 0);
        
    }

    else
    {
        SetMotorVoltage(speed + 0.006 * Error, speed - 0.006 * Error);
        //SetMotorVoltage(speedl + 0.0015 * Error, speedr - 0.0015 * Error);
    }
}