Diff: Strategie/Strategie.cpp

Revision:

82:759ceb2a8a70

Parent:

81:ef50ec0ef328

Child:

83:23e04b85ae06

--- a/Strategie/Strategie.cpp	Wed May 29 21:27:00 2019 +0000
+++ b/Strategie/Strategie.cpp	Thu May 30 11:50:05 2019 +0000
@@ -1203,7 +1203,7 @@
                     if(InversStrat == 1 && ingnorInversionOnce == 0) {
                         localData2 = -localData2;
                     }
-                    
+
 
                     if(instruction.direction == ABSOLUTE) {
                         //C'est un rotation absolu, il faut la convertir en relative
@@ -1534,7 +1534,7 @@
                     }
                     ingnorBaliseOnce = 0;
                     ingnorBalise = 0;
-                    
+
                     debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
                     waitingAckID = ASSERVISSEMENT_XYT;
                     waitingAckFrom = ACKNOWLEDGE_MOTEUR;
@@ -1548,15 +1548,15 @@
                     GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
                     //gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
                     break;
-                    
-                    
-                    //
-                    //return;
+
+
+                //
+                //return;
 
                 case MV_XYT:
                     ingnorBaliseOnce = 0;
                     ingnorBalise = 0;
-                    
+
                     gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
                     ///////cv
                     break;
@@ -1566,13 +1566,13 @@
                     //instruction.arg3 =  instruction.arg3 - target_theta_robot;
                     ingnorBaliseOnce = 0;
                     ingnorBalise = 0;
-                    
+
                     if(instruction.direction == LEFT) {
                         target_theta_robot = target_theta_robot - theta_robot;
                     } else {
                         target_theta_robot = theta_robot + target_theta_robot;
                     }
-                    
+
                     target_theta_robot = (target_theta_robot)%3600;
                     if(target_theta_robot > 1800) {
                         target_theta_robot = target_theta_robot-3600;
@@ -1585,7 +1585,7 @@
                     gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
 
                     break;
-                    
+
                 default:
                     actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                     gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
@@ -2122,8 +2122,8 @@
                             waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
                             while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
                                 canProcessRx();*/
-                            //while(1); // ligne à décommenter si on est en homologation
-                            
+                            //while(1); // ligne à décommenter si on est en homologation 2018
+
                             if(gameEtat != ETAT_WARING_END_BALISE_WAIT  || 1) {
                                 timeoutWarning.reset();
                                 timeoutWarning.start();//Reset du timer utiliser par le timeout
@@ -2131,8 +2131,22 @@
                             gameEtat = ETAT_WARNING_TIMEOUT;
                         }
                     }
+                    ingnorBaliseOnce = 0;
+
+                } else if(gameEtat==ETAT_WARNING_TIMEOUT) {
+                    if (!(instruction.order == MV_LINE && instruction.direction == FORWARD  && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+                            || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
+                            || instruction.order == MV_COURBURE && direction == 1 && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
+                            || instruction.order == MV_COURBURE && direction == -1 && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere
+                            || instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant  && angle_moyen_balise_IR <= seuil_haut_avant
+                            || instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere  && angle_moyen_balise_IR <= seuil_haut_arriere) ) {
+
+                        timeoutWarningWaitEnd.reset();
+                        timeoutWarningWaitEnd.start();
+                        gameEtat = ETAT_WARING_END_BALISE_WAIT;
+                    }
                 }
-                ingnorBaliseOnce = 0;
+
                 break;
 
             case BALISE_END_DANGER:
@@ -2142,7 +2156,7 @@
                     timeoutWarningWaitEnd.start();
                     gameEtat = ETAT_WARING_END_BALISE_WAIT;
                 }
-                
+
                 break;