/******************************************************************************* * 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); }
/******************************************************************************* * 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)); //加入死区电压 } }
/******************************************************************************* * 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); }
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); }
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); } } }
/************电机输出函数*************/ 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 } //右电机反转 }
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); }
/********************************舵机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); } }