|
|
這個(gè)掉頭是在超聲波模塊檢測(cè)到障礙物距離在20----30厘米時(shí)，標(biāo)志位置1，之后在執(zhí)行程序里將標(biāo)志位置0，但是每次都是掉頭兩次，本來(lái)掉一次頭就能轉(zhuǎn)180度了，但是兩次掉頭就變成還是面向障礙物。
讀取距離標(biāo)志：Read_Flag_ultra_Avg，速度PID計(jì)算也在這里。
各位大神幫忙看一下，這個(gè)掉頭的程序： if(Stop_Turn_Flag==1)，在Stop_Turn_Flag=1時(shí)，每次都是執(zhí)行兩次，這是為什么啊？
單片機(jī)源程序如下:
- 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)則掉頭，否則就執(zhí)行繼續(xù)前行程序
- 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;
- }
-
-
-
-
- }
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
