Diff: main.cpp

Revision:

36:0a6cb92024c7

Parent:

35:6c6321b97ae6

Child:

37:65650aab8387

--- a/main.cpp	Fri Jun 07 15:46:27 2019 +0000
+++ b/main.cpp	Sat Jun 08 00:57:21 2019 +0000
@@ -5,15 +5,14 @@
 
 bool automate_aveugle(Robot&, bool&);       //automate de début de partie
 void automate_ejecte(Robot&, bool&);        //
-void testLasers(Robot&);           //
+void automate_testLasers(Robot&);           //
 void automate_testABalle(Robot&, bool&);    //
 void automate_deplacement(Robot&);          //
-void automate_vitesse(Robot&);              //automate pour la vitesse du robot
 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
 
-typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT} type_etat_deplacement;
+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;
 
 
@@ -21,15 +20,13 @@
 {
     Robot robot;
     
-    dbug.printf("%d\n\r",etat_deplacement);
-    
     while(1)
         automate_run(robot);   
 }
 
 void automate_run(Robot& robot)
 {
-    bool ejecte = false;
+    static bool ejecte = false;
     typedef enum{ATTENTE, RUN_AVEUGLE, RECHERCHE_BALLES, FIN_PARTIE} type_etat;
     static type_etat etat = ATTENTE;
     static Timer T_partie;
@@ -43,7 +40,7 @@
             if(!Robot::Jack)
             {
                 T_partie.start();
-                etat = RUN_AVEUGLE;
+                etat = RECHERCHE_BALLES;
             }
             break;
             
@@ -54,17 +51,18 @@
             break;
             
         case RECHERCHE_BALLES:
-            // Gestion du lancer de balle
-            automate_testABalle(robot, ejecte);
-            automate_ejecte(robot, ejecte);
             
-            // Recherche de balles
-            testLasers(robot);
             if(!robot.aBalle())
                 automate_deplacement(robot);
             
-            // Gestion de la vitesse en fonction de l'état actuel
-            automate_vitesse(robot);
+            // Recherche de balles
+            automate_testLasers(robot);
+            
+            // Gestion du lancer de balle
+            automate_testABalle(robot, ejecte);
+            
+            //Ejecte ou non en fonction des commandes des autres automates
+            automate_ejecte(robot, ejecte);
             break;
             
         case FIN_PARTIE:
@@ -73,31 +71,127 @@
     }
 }
 
-void testLasers(Robot& robot)
-{   
-    static float past_droit, actual_droit;
+void automate_testLasers(Robot& robot)
+{      
+    typedef enum{RAS, SUSPICION, CONFIRMATION, VERIFICATION, CORRIGE, ABALLE, ATTENTE_VISION} type_etat;
+    static type_etat etat = RAS;
     
-    actual_droit = robot.getDist(Laser::Droit);
+    static const int distMurBalle = 20;
+    static Timer T_suspi, T_arret, T_balle;
+    static float droit[2] =  { robot.getDist(Laser::Droit),robot.getDist(Laser::Droit) }, 
+                 gauche[2] = { robot.getDist(Laser::Gauche),robot.getDist(Laser::Gauche) },
+                 distBalle;
+    
+    droit[0] = robot.getDist(Laser::Droit);
+    gauche[0] = robot.getDist(Laser::Gauche);
+    
+    if(robot.aBalle())
+        etat = ABALLE;
     
-    dbug.printf("%f\n\r",actual_droit);
-    if(actual_droit < past_droit-30 && actual_droit <= 130 && etat_deplacement == TURN_FAST)
+    switch(etat)
     {
-        robot.stop();
-        etat_deplacement = RETROUVE;
-    }
-    else if(actual_droit < past_droit-30 && etat_deplacement == RETROUVE)
-    {
-        robot.stop();
-        etat_deplacement = GO_STRAIGHT;
+        case RAS:
+            if(etat_deplacement != TURN_FAST_GO)
+                etat_deplacement = TURN_FAST;
+            if( droit[1] > (droit[0] + distMurBalle) && droit[0] <= 100)
+            {
+                T_suspi.start();
+                etat = SUSPICION;
+                dbug.printf("SUSPICION\n\r");
+            }
+            break;
+            
+        case SUSPICION:
+            if(T_suspi.read_ms() < 500)
+            {
+                if( gauche[1] > (gauche[0] + distMurBalle) )
+                {
+                    etat = CONFIRMATION;
+                    dbug.printf("CONFIRMATION\n\r");
+                    T_suspi.stop();
+                    T_suspi.reset();
+                }
+            }
+            else
+            {
+                T_suspi.stop();
+                T_suspi.reset();
+                etat = RAS;
+                dbug.printf("RAS\n\r");
+            }
+            break;
+            
+        case CONFIRMATION:
+            distBalle = gauche[0];
+            robot.stop();
+            T_arret.start();
+            etat = VERIFICATION;
+            dbug.printf("VERIFICATION\n\r");
+            break;
+            
+        case VERIFICATION:
+            if(T_arret.read_ms() > 300)
+            {
+                T_arret.stop();
+                T_arret.reset();
+                
+                if( fabs(gauche[0] - droit[0]) <= 5.5f && fabs(gauche[0]-distBalle) <= 6.0f)
+                {
+                    if(etat_deplacement != STRAIGHT_GO)
+                    {
+                        etat_deplacement = STRAIGHT;
+                        dbug.printf("STRAIGHT\n\r");
+                    }
+                }
+                else
+                {
+                    etat_deplacement = CORRECT_GAUCHE;
+                    etat = CORRIGE;
+                    dbug.printf("CORRIGE\n\r");
+                }
+            }
+            break;
+            
+        case CORRIGE:
+            if( droit[1] > (droit[0] + distMurBalle) )
+            {
+                if(etat_deplacement != STRAIGHT_GO)
+                {
+                    etat_deplacement = STRAIGHT;
+                    dbug.printf("STRAIGHT\n\r");
+                }
+            }
+            break;
+            
+        case ABALLE:
+            if(!robot.aBalle())
+            {
+                T_balle.start();
+                etat = ATTENTE_VISION;
+                dbug.printf("ATTENTE_VISION\n\r");
+                etat_deplacement = TURN_FAST;
+            }
+            break;
+            
+        case ATTENTE_VISION:
+            if(T_balle.read() > 1.0f)
+            {
+                T_balle.stop();
+                T_balle.reset();
+                etat = RAS;
+                dbug.printf("RAS\n\r");
+            }
+            break;
     }
     
-    
-    past_droit = actual_droit;
+    // Conversions past
+    droit[1] = droit[0];
+    gauche[1] = gauche[0];
 }
 
 void automate_testABalle(Robot& robot, bool& ejecte)
 {
-    typedef enum {ATTENTE,ABALLE} type_etat;
+    typedef enum {ATTENTE, ABALLE} type_etat;
     static type_etat etat = ATTENTE;
     
     switch(etat)
@@ -107,7 +201,7 @@
             {
                 robot.stop();
                 etat = ABALLE;
-                robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
+                robot.setSpeed(150,800); //vitesse de rotation vers le terrain ennemi
             }
             break;
             
@@ -125,35 +219,38 @@
 void automate_deplacement(Robot& robot)
 {    
     switch(etat_deplacement)
-    {
+    {            
         case TURN_FAST:
-            robot.tourne();
+            robot.stop();
+            robot.setSpeed(90,450);
+            etat_deplacement = TURN_FAST_GO;
             break;
             
-        case RETROUVE:
-            robot.tourne(-30);
+        case TURN_FAST_GO:
+            if(robot.immobile())
+                robot.tourne();
             break;
             
-        case GO_STRAIGHT:
-            robot.avance(robot.getDist(Laser::Droit,"mm"));
-            break;
-    }
-}
-
-void automate_vitesse(Robot& robot)
-{
-    switch(etat_deplacement)
-    {
-        case TURN_FAST:
-            robot.setSpeed(150,200);
+        case STRAIGHT:
+            robot.stop();
+            robot.setSpeed(150,800);
+            etat_deplacement = STRAIGHT_GO;
             break;
             
-        case RETROUVE:
-            robot.setSpeed(100,300);
+        case STRAIGHT_GO:
+            if(robot.immobile())
+                robot.avance( robot.getDist(Laser::Gauche,"mm") );
             break;
             
-        case GO_STRAIGHT:
-            robot.setSpeed(320,1600);
+        case CORRECT_GAUCHE:
+            robot.stop();
+            robot.setSpeed(35,450);
+            etat_deplacement = CORRECT_GAUCHE_GO;
+            break;
+            
+        case CORRECT_GAUCHE_GO:
+            if(robot.immobile())
+                robot.tourne(-100);
             break;
     }
 }
@@ -168,8 +265,7 @@
     switch(etat)
     {
         case AVANCE:
-            robot.stopRouleau();
-            if(robot.GoToXYT(2000,3800,0))
+            if(robot.GoToXYT(robot.pos(Robot::X),3600,0))
                 etat = EXPLOSE_BALLON;
             break;
             
@@ -186,6 +282,7 @@
             if(T_fin.read() >= 2.0f && robot.baisseBras())
             {
                 robot.arreteDisquette();
+                robot.stopRouleau();
                 return true;
             }           
     }