//100ms
void TIM7_IRQHandler(void)
{
float pwm1, pwm2;
if(TIM_GetITStatus(TIM7, TIM_IT_Update) == SET)
{
TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
//printf("counter: %d, %d\r\n", left_counter, right_counter);
if(control_flag == 1 && adc_flag == 1){
if(pos_flag == 1){
pwm1 = 0;
pwm2 = 0;
left_pos_counter += left_counter;
right_pos_counter += right_counter;
if(left_pos_counter < left_pos_target_counter){
pwm1 = 90*left_dir;
}
if(right_pos_counter < right_pos_target_counter){
pwm2 = 90*right_dir;
}
motor_set_pwm(pwm1, pwm2);
//printf("counter:%d,%d; pos:%d,%d; target:%d,%d; pwm:%6.1f,%6.1f\r\n", left_counter, right_counter, left_pos_counter, right_pos_counter, left_pos_target_counter, right_pos_target_counter, pwm1, pwm2);
if(pwm1==0&&pwm2==0){
left_dir = 1;
right_dir = 1;
pos_flag = 0;
}
}
else{
left_speed_pid.feedback = left_counter;
right_speed_pid.feedback = right_counter;
left_pwm += pid_calc(&left_speed_pid);
right_pwm += pid_calc(&right_speed_pid);
left_pwm = range(left_pwm, 0, 100);
right_pwm = range(right_pwm, 0, 100);
motor_set_pwm(left_pwm*left_dir, right_pwm*right_dir);
}
}
else{
motor_set_pwm(0, 0);
}
left_counter = 0;
right_counter = 0;
}
}
void side_to_side(void)
{
static int8_t dir_flag = 0;
float Ep, Ev;
while(1){
if(x4*cos(x2*PI/180.0f) < 0){
dir_flag = 1; //Õýת
}
else{
dir_flag = -1; //·´×ª
}
Ep = 0.004173f;
Ev = 0.000356f*(x4*PI/180)*(x4*PI/180)+0.002082f*(1+cos(x2*PI/180.0f));
//pwm = -dir_flag*(Ep-Ev)/Ep*PWM_GAIN;
pwm = -dir_flag*20;
//printf("x2=%f, x4=%f, dir_flag=%d, Ep=%f, Ev=%f, pwm=%f\r\n", x2, x4, dir_flag, Ep, Ev, pwm);
delay_ms(100);
motor_set_pwm(pwm);
}
}
