这个掉头是在超声波模块检测到障碍物距离在20----30厘米时，标志位置1，之后在执行程序里将标志位置0，但是每次都是掉头两次，本来掉一次头就能转180度了，但是两次掉头就变成还是面向障碍物。
读取距离标志：Read_Flag_ultra_Avg，速度PID计算也在这里。
各位大神帮忙看一下，这个掉头的程序： if(Stop_Turn_Flag==1)，在Stop_Turn_Flag=1时，每次都是执行两次，这是为什么啊？
单片机源程序如下:
- s16 Read_Flag_ultra_Avg=0,Stop_Flag=0;
- void TIM7_IRQHandler(void)
- {
- if (TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
- {
-
- TIM_ClearITPendingBit(TIM7, TIM_IT_Update );
- tim_motor_flag=1;
- Cnt++;
- Read_Flag_ultra_Avg++;
- if(Cnt>=5)
- {
- Cnt=0;
- Read_Flag_ultra_Avg=1;
- }
- LeftWheelSpeed= Motor_GetLeftEncResult();
- LeftPIDResult = PID_LocPIDCalc(&PID_LeftWheel,(float)Left_Desired_Speed,(float)LeftWheelSpeed,1);
- LeftPWMOut = LeftPIDResult;
- LeftPWMOut = LeftPWMOut/5;
- Motor_SetLeftPWM(LeftPWMOut);
-
- RightWheelSpeed=Motor_GetRightEncResult();
- RightPIDResult = PID_LocPIDCalc(&PID_RightWheel,(float)Right_Desired_Speed,(float)RightWheelSpeed,1);
- RightPWMOut = RightPIDResult;
- RightPWMOut = RightPWMOut/5;
- Motor_SetRightPWM(RightPWMOut);
- }
- }
- 主程序判断障碍物距离20cm-----30cm就将Stop_Turn_Flag置1
- if (Read_Flag_ultra_Avg==1)
- {
- Read_Flag_ultra_Avg=0;
- ultra_Avg=ultrasonic_d();
- // printf("ultra_Avg=%.2f\r\n",ultra_Avg
- if(ultra_Avg>=20&&ultra_Avg<=30)
- {
- // printf("ultra_Avg_COM=%.2f\r\n",ultra_Avg);
- Stop_Flag=1;
- Go_Flag=0;
- Stop_Turn_Flag=1;
- }
- if (ultra_Avg<=20||ultra_Avg>=30)
- {
- Stop_Flag=0;
- Go_Flag=1;
- Stop_Turn_Flag=0;
- }
- }
- 子程序，如果 if(Stop_Turn_Flag==1)则掉头，否则就执行继续前行程序
- if(Stop_Turn_Flag==1)
- {
- // Stop_Turn_Flag=0;
- Right_Desired_Speed=0;
- Left_Desired_Speed=0;
- delay_ms(999);
- delay_ms(999);
- // delay_ms(999
-
- Right_Desired_Speed=400;
- Left_Desired_Speed=-400;
- LED4 =!LED4;
- delay_ms(599);
-
- Go_Flag=1;
- Stop_Turn_Flag=0;
- // printf("Stop_Turn_Flag=%d\r\n",Stop_Turn_Flag);
- // printf("Go_Flag=%d\r\n",Go_Flag
- Read_Flag_ultra_Avg=1;
- }
- if(Go_Flag==1)
- {
- Stop_Flag=0;
- Go_Flag=0;
- Stop_Turn_Flag=0;
- {
- Actual_N = Extracting_BlackLIne() ;
- Error_N = Actual_N - 58 ;
- if(Error_N < -4)
- {
- Left_Desired_Speed = 300+ Error_N*1;
- Right_Desired_Speed = 300- Error_N*1;
- if(Right_Desired_Speed > 600) Right_Desired_Speed = 600 ;
- if(Left_Desired_Speed < 0) Left_Desired_Speed = 0 ;
- }
- if(Error_N > 4)
- {
- Left_Desired_Speed = 300 + Error_N*1;
- Right_Desired_Speed = 300 - Error_N*1;
- if(Left_Desired_Speed > 600) Left_Desired_Speed = 600 ;
- if(Right_Desired_Speed < 0) Right_Desired_Speed = 0 ;
- }
- else
- {
- Left_Desired_Speed = 300;
- Right_Desired_Speed = 300;
- }
-
-
-
-
- }
|
