Diff: main.cpp

Revision:

29:6bce50d6530c

Parent:

28:82571fd665bf

Child:

31:85d9fb71f921

--- a/main.cpp	Thu Jun 06 16:01:05 2019 +0000
+++ b/main.cpp	Fri Jun 07 09:30:46 2019 +0000
@@ -1,12 +1,16 @@
 #include "mbed.h"
 #include "CAN_asser.h"
 #include "Robot.h"
+#include "math.h"
 
-bool automate_aveugle(Robot&);
+bool automate_aveugle(Robot&, bool&);
+void automate_ejecte(Robot&, bool&);
 void automate_testLasers(Robot&);
-void automate_testABalle(Robot&);
+void automate_testABalle(Robot&, bool&);
 void automate_deplacement(Robot&);
 void automate_vitesse(Robot&);
+void automate_fin_de_partie(Robot&);
+bool automate_arretUrgence(Robot&);
 
 typedef enum{TURN_FAST, CORRECT_GAUCHE, CORRECT_DROITE, GO_STRAIGHT} type_etat_deplacement;
 type_etat_deplacement etat_deplacement = TURN_FAST;
@@ -16,85 +20,191 @@
 {
     Robot robot;
     
-    robot.setSpeed(80,450); // vitesse de rotation de recherche de la balle
+    bool ejecte = false;
     
     while(1)
     if(!Robot::Jack)
     {
-        while(!automate_aveugle(robot));
+        // Trois balles en début de partie à l'aveugle
+        while( !automate_aveugle(robot, ejecte) );
+            
+        automate_ejecte(robot, ejecte);
+            
+        automate_fin_de_partie(robot);
         
-        automate_testABalle(robot);
+        // Gestion du lancer de balle
+        /*automate_testABalle(robot, ejecte);
+        automate_ejecte(robot, ejecte);
         
+        // Recherche de balles
         automate_testLasers(robot);
-        
         if(!robot.aBalle())
             automate_deplacement(robot);
-            
-        automate_vitesse(robot);
+        
+        // Gestion de la vitesse en fonction de l'état actuel
+        automate_vitesse(robot);*/
     }
 }
 
-bool automate_aveugle(Robot& robot)
+void automate_fin_de_partie(Robot& robot)
 {
-    typedef enum{STRAIGHT1, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
-    static type_etat etat = STRAIGHT1;
+    typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
+    static type_etat etat = AVANCE;
     
-    static Timer T_ejecte;
+    static Timer T_fin;
     
     switch(etat)
     {
-        case STRAIGHT1:
+        case AVANCE:
             robot.avance();
-            if(robot.aBalle())
+            if(automate_arretUrgence(robot))
+                etat = EXPLOSE_BALLON;
+            break;
+            
+        case EXPLOSE_BALLON:
+            robot.stop();
+            if(robot.leveBras())
             {
-                robot.stop();
-                robot.ejecte();
-                etat = TOURNE_D;
-            }
-            else if( T_ejecte.read() >= 1.0f )
-            {
-                robot.gobe();
-                T_ejecte.stop();
-                T_ejecte.reset();
+                robot.exploseBallon();
+                T_fin.start();
+                etat = FIN_PARTIE;
             }
             break;
             
-        case TOURNE_D:
-            if( T_ejecte.read() >= 1.0f )
+        case FIN_PARTIE:
+            if(T_fin.read() >= 2.0f)
             {
-                robot.gobe();
-                T_ejecte.stop();
-                T_ejecte.reset();
+                robot.baisseBras();
+                robot.arreteDisquette();
             }
-                
-            if(robot.tourne(900))
-                etat = STRAIGHT2;
-            break;
-            
-        case STRAIGHT2:
-            robot.avance();
-            if(robot.aBalle())
-            {
-                robot.stop();
-                etat = TOURNE_G;
+            break;            
+    }
+}
+
+bool automate_arretUrgence(Robot& robot)
+{
+    typedef enum{RAS, PERCEPTION, ARRET_URGENCE, ATTENTE_REPLACEMENT} type_etat;
+    static type_etat etat = RAS;
+    
+    // Timer pour la durée sur la ligne blanche
+    static Timer timerCNY;
+    
+    switch(etat)
+    {
+        case RAS :
+            if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT )){
+                etat = PERCEPTION;
+                timerCNY.start();
             }
             break;
             
-        case TOURNE_G:
-            if(robot.tourne(-900))
-                robot.ejecte();
-                
-            if(!robot.aBalle())
-            {
-                T_ejecte.start();
-                etat = TOURNE_D;
-            } 
+        case PERCEPTION :
+            if( robot.surBlanc( Robot::CNY_GAUCHE ) && robot.surBlanc( Robot::CNY_DROIT ) && timerCNY.read() >= 0.15f )
+                etat = ARRET_URGENCE;
+            else if( timerCNY.read() >= 0.15f ){
+                etat = RAS;
+                timerCNY.stop();
+                timerCNY.reset();
+            }
+            break;
+            
+        case ARRET_URGENCE :
+            timerCNY.stop();
+            timerCNY.reset();
+            
+            etat = ATTENTE_REPLACEMENT;
+            
+            return true;
+            
+        case ATTENTE_REPLACEMENT :
+            if( !robot.surBlanc( Robot:: CNY_GAUCHE )  && !robot.surBlanc( Robot::CNY_DROIT ))
+                etat = RAS;
             break;
     }
     
     return false;
 }
 
+bool automate_aveugle(Robot& robot, bool& ejecte)
+{
+    typedef enum{STRAIGHT1, PREMIER_TOURNE_D, TOURNE_D, STRAIGHT2, TOURNE_G} type_etat;
+    static type_etat etat = STRAIGHT1;
+    
+    static Timer T_ejecte;
+    static int cptBalles = 0;
+    
+    if(cptBalles == 2)
+        return true;
+    
+    switch(etat)
+    {
+        case STRAIGHT1:
+            if(robot.avance(1850 - DIST_CENTRE))
+            {
+                ejecte = true;
+                etat = PREMIER_TOURNE_D;
+            }
+            break;
+            
+        case PREMIER_TOURNE_D:
+            if(robot.tourne(ANGLE_DROIT))
+                etat = STRAIGHT2;
+            break;
+            
+        case TOURNE_D:                
+            if(robot.tourne(ANGLE_DROIT))
+                etat = STRAIGHT2;
+            break;
+            
+        case STRAIGHT2:
+            if(robot.avance(1500))
+                etat = TOURNE_G;
+            break;
+            
+        case TOURNE_G:
+            if(robot.tourne(-ANGLE_DROIT))
+            {
+                ejecte = true;
+                cptBalles++;
+                etat = TOURNE_D;
+            }
+            break;
+    }
+    
+    return false;
+}
+
+void automate_ejecte(Robot& robot, bool& ejecte)
+{
+    typedef enum{GOBE, EJECTE, ATTENTE} type_etat;
+    static type_etat etat = GOBE;
+    
+    static Timer timer_ejecte;
+    
+    switch(etat)
+    {
+        case GOBE:
+            robot.gobe();
+            if(ejecte)
+                etat = EJECTE;
+            break;
+            
+        case EJECTE:
+            robot.ejecte();
+            timer_ejecte.start();
+            etat = ATTENTE;
+            break;
+            
+        case ATTENTE:
+            if( timer_ejecte >= 1.0f ){
+                timer_ejecte.stop();
+                timer_ejecte.reset();
+                ejecte = false;
+                etat = GOBE;   
+            }
+            break;
+    }
+}
 
 void automate_testLasers(Robot& robot)
 {   
@@ -132,13 +242,11 @@
     past_droit = actual_droit;
 }
 
-void automate_testABalle(Robot& robot)
+void automate_testABalle(Robot& robot, bool& ejecte)
 {
     typedef enum {ATTENTE,ABALLE} type_etat;
     static type_etat etat = ATTENTE;
     
-    static Timer T_ejecte;
-    
     switch(etat)
     {
         case ATTENTE:
@@ -148,24 +256,15 @@
                 etat = ABALLE;
                 robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
             }
-            else if( T_ejecte.read() >= 1.0f )
-            {
-                robot.gobe();
-                T_ejecte.stop();
-                T_ejecte.reset();
-            }
             break;
             
         case ABALLE:
-            if(robot.tourne(1800))
-                robot.ejecte();
-                
-            if(!robot.aBalle())
+            if( robot.tourne( -robot.pos(Robot::THETA) ) )
             {
-                T_ejecte.start();
+                ejecte = true;
                 etat = ATTENTE;
                 etat_deplacement = TURN_FAST;
-            }   
+            }
             break;
     }
 }