/*******************************************************************************
* 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);
}
}