Diff: main.cpp

Revision:

37:65650aab8387

Parent:

36:0a6cb92024c7

Child:

38:a8b84be7dce5

--- a/main.cpp	Sat Jun 08 00:57:21 2019 +0000
+++ b/main.cpp	Sat Jun 08 02:47:31 2019 +0000
@@ -11,17 +11,21 @@
 bool automate_fin_de_partie(Robot&);        //automate crève ballon de fin de partie
 bool automate_arretUrgence(Robot&);         //automate d'arrêt d'urgence au filet
 void automate_run(Robot&);                  //automate principale de partie
+void securitePos(Robot&);
 
 typedef enum{TURN_FAST, TURN_FAST_GO, STRAIGHT, STRAIGHT_GO, CORRECT_GAUCHE, CORRECT_GAUCHE_GO} type_etat_deplacement;
 type_etat_deplacement etat_deplacement = TURN_FAST;
 
+bool forceRAS = false;
 
 int main(void)
 {
     Robot robot;
     
     while(1)
-        automate_run(robot);   
+    {
+        automate_run(robot);
+    }   
 }
 
 void automate_run(Robot& robot)
@@ -32,7 +36,13 @@
     static Timer T_partie;
     
     if(T_partie.read() > 80.0f)
+    {
         etat = FIN_PARTIE;
+        robot.stop();
+        robot.ejecte();
+        T_partie.stop();
+        T_partie.reset();
+    }
     
     switch(etat)
     {
@@ -40,7 +50,7 @@
             if(!Robot::Jack)
             {
                 T_partie.start();
-                etat = RECHERCHE_BALLES;
+                etat = RUN_AVEUGLE;
             }
             break;
             
@@ -52,7 +62,10 @@
             
         case RECHERCHE_BALLES:
             
-            if(!robot.aBalle())
+            //le robot ne s'approche pas des murs
+            securitePos(robot);
+            
+            if(!robot.aBalle() && !forceRAS)
                 automate_deplacement(robot);
             
             // Recherche de balles
@@ -66,7 +79,7 @@
             break;
             
         case FIN_PARTIE:
-            automate_fin_de_partie(robot);  
+            automate_fin_de_partie(robot);
             break; 
     }
 }
@@ -77,7 +90,7 @@
     static type_etat etat = RAS;
     
     static const int distMurBalle = 20;
-    static Timer T_suspi, T_arret, T_balle;
+    static Timer T_suspi, T_arret, T_balle, T_corrige;
     static float droit[2] =  { robot.getDist(Laser::Droit),robot.getDist(Laser::Droit) }, 
                  gauche[2] = { robot.getDist(Laser::Gauche),robot.getDist(Laser::Gauche) },
                  distBalle;
@@ -87,13 +100,16 @@
     
     if(robot.aBalle())
         etat = ABALLE;
+        
+    if(forceRAS)
+        etat = RAS;
     
     switch(etat)
     {
         case RAS:
             if(etat_deplacement != TURN_FAST_GO)
                 etat_deplacement = TURN_FAST;
-            if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 100)
+            if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 150)
             {
                 T_suspi.start();
                 etat = SUSPICION;
@@ -146,6 +162,7 @@
                 else
                 {
                     etat_deplacement = CORRECT_GAUCHE;
+                    T_corrige.start();                  //Timer pour abandonner en cas de non retrouvage de balle
                     etat = CORRIGE;
                     dbug.printf("CORRIGE\n\r");
                 }
@@ -159,8 +176,16 @@
                 {
                     etat_deplacement = STRAIGHT;
                     dbug.printf("STRAIGHT\n\r");
+                    T_corrige.stop();
+                    T_corrige.reset();
                 }
             }
+            else if( T_corrige.read() > 1.5f )
+            {
+                etat = RAS;
+                T_corrige.stop();
+                T_corrige.reset();    
+            }
             break;
             
         case ABALLE:
@@ -189,6 +214,35 @@
     gauche[1] = gauche[0];
 }
 
+void securitePos(Robot& robot)
+{
+    static int etat;
+    
+    switch(etat)
+    {
+        case 0:
+            if(robot.pos(Robot::X) < 400 || robot.pos(Robot::X) > 3600 || robot.pos(Robot::Y) < 400 || robot.pos(Robot::Y) > 3500)
+            {
+                forceRAS = true;
+                robot.stop();
+                etat = 1;
+                dbug.printf("GoToXYT\n\r");
+            }
+            else
+                forceRAS = false;
+            break;
+            
+        case 1:
+            if(robot.GoToXYT(2000,2000,0))
+            {
+                forceRAS = false;
+                etat = 0;
+                dbug.printf("Remis au milieu\n\r");
+            }
+            break;
+    }
+}
+
 void automate_testABalle(Robot& robot, bool& ejecte)
 {
     typedef enum {ATTENTE, ABALLE} type_etat;