Diff: main.cpp

Revision:

35:6c6321b97ae6

Parent:

34:cea05fa02f37

Child:

36:0a6cb92024c7

--- a/main.cpp	Fri Jun 07 14:20:02 2019 +0000
+++ b/main.cpp	Fri Jun 07 15:46:27 2019 +0000
@@ -5,7 +5,7 @@
 
 bool automate_aveugle(Robot&, bool&);       //automate de début de partie
 void automate_ejecte(Robot&, bool&);        //
-void automate_testLasers(Robot&);           //
+void testLasers(Robot&);           //
 void automate_testABalle(Robot&, bool&);    //
 void automate_deplacement(Robot&);          //
 void automate_vitesse(Robot&);              //automate pour la vitesse du robot
@@ -13,7 +13,7 @@
 bool automate_arretUrgence(Robot&);         //automate d'arrêt d'urgence au filet
 void automate_run(Robot&);                  //automate principale de partie
 
-typedef enum{TURN_FAST, CORRECT_GAUCHE, CORRECT_DROITE, GO_STRAIGHT} type_etat_deplacement;
+typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT} type_etat_deplacement;
 type_etat_deplacement etat_deplacement = TURN_FAST;
 
 
@@ -21,6 +21,8 @@
 {
     Robot robot;
     
+    dbug.printf("%d\n\r",etat_deplacement);
+    
     while(1)
         automate_run(robot);   
 }
@@ -46,7 +48,8 @@
             break;
             
         case RUN_AVEUGLE:
-            automate_aveugle(robot, ejecte);
+            if(automate_aveugle(robot, ejecte) && robot.GoToXYT(2000,2500,0))
+                etat = RECHERCHE_BALLES;
             automate_ejecte(robot, ejecte);
             break;
             
@@ -56,7 +59,7 @@
             automate_ejecte(robot, ejecte);
             
             // Recherche de balles
-            automate_testLasers(robot);
+            testLasers(robot);
             if(!robot.aBalle())
                 automate_deplacement(robot);
             
@@ -65,154 +68,30 @@
             break;
             
         case FIN_PARTIE:
-            T_partie.stop();
-            robot.stopRouleau();
+            automate_fin_de_partie(robot);  
             break; 
     }
 }
 
-bool automate_fin_de_partie(Robot& robot)
-{
-    typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
-    static type_etat etat = AVANCE;
-    
-    static Timer T_fin;
-    
-    switch(etat)
-    {
-        case AVANCE:
-            if(robot.GoToXYT(3500,3800,0))
-                etat = EXPLOSE_BALLON;
-            break;
-            
-        case EXPLOSE_BALLON:
-            if(robot.leveBras())
-            {
-                robot.exploseBallon();
-                T_fin.start();
-                etat = FIN_PARTIE;
-            }
-            break;
-            
-        case FIN_PARTIE:
-            if(T_fin.read() >= 2.0f && robot.baisseBras())
-            {
-                robot.arreteDisquette();
-                return true;
-            }           
-    }
-    
-    return false;
-}
-
-bool automate_aveugle(Robot& robot, bool& ejecte)
-{
-    typedef enum{STRAIGHT1, TOURNE_G, STRAIGHT2, STRAIGHT3, FIN_AVEUGLE} type_etat;
-    static type_etat etat = STRAIGHT1;
+void testLasers(Robot& robot)
+{   
+    static float past_droit, actual_droit;
     
-    switch(etat)
-    {
-        case STRAIGHT1:
-            if(robot.avance(1850 - DIST_CENTRE))
-                etat = TOURNE_G;
-            break;
-            
-        case TOURNE_G:
-            if( robot.tourne(-100) )
-            {
-                ejecte = true;
-                etat = STRAIGHT2;
-            }
-            break;
-            
-        case STRAIGHT2:
-            if( robot.GoToXYT(2000, 1850, 180) )
-            {
-                ejecte = true;
-                etat = STRAIGHT3;
-            }
-            break;
-            
-        case STRAIGHT3:
-            if( robot.GoToXYT(3500, 1850, 50) )
-            {
-                ejecte = true;
-                etat = FIN_AVEUGLE;
-            }
-            break;
-            
-        case FIN_AVEUGLE:
-            return true;
-    }
-    
-    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)
-{   
-    static const int diffMurBalle = 20;
-    static float past_gauche, past_droit, actual_gauche, actual_droit;
-    static float distBalle;
-    
-    actual_gauche = robot.getDist(Laser::Gauche);
     actual_droit = robot.getDist(Laser::Droit);
     
-    switch(etat_deplacement)
+    dbug.printf("%f\n\r",actual_droit);
+    if(actual_droit < past_droit-30 && actual_droit <= 130 && etat_deplacement == TURN_FAST)
     {
-        case TURN_FAST:
-            if(actual_droit < past_droit - diffMurBalle)
-            {
-                distBalle = actual_droit;
-                robot.stop();
-                etat_deplacement = CORRECT_GAUCHE;
-            }
-            break;
-            
-        case CORRECT_GAUCHE:
-            
-            break;
-            
-        case CORRECT_DROITE:
-            break;
-            
-        case GO_STRAIGHT:
-            break;
+        robot.stop();
+        etat_deplacement = RETROUVE;
+    }
+    else if(actual_droit < past_droit-30 && etat_deplacement == RETROUVE)
+    {
+        robot.stop();
+        etat_deplacement = GO_STRAIGHT;
     }
     
     
-    past_gauche = actual_gauche;
     past_droit = actual_droit;
 }
 
@@ -251,16 +130,12 @@
             robot.tourne();
             break;
             
-        case CORRECT_GAUCHE:
-            robot.tourne(-450);
-            break;
-            
-        case CORRECT_DROITE:
-            robot.tourne(450);
+        case RETROUVE:
+            robot.tourne(-30);
             break;
             
         case GO_STRAIGHT:
-            robot.avance();
+            robot.avance(robot.getDist(Laser::Droit,"mm"));
             break;
     }
 }
@@ -270,19 +145,131 @@
     switch(etat_deplacement)
     {
         case TURN_FAST:
-            robot.setSpeed(80,450);
+            robot.setSpeed(150,200);
             break;
             
-        case CORRECT_GAUCHE:
-            robot.setSpeed(50,300);
-            break;
-            
-        case CORRECT_DROITE:
-            robot.setSpeed(50,300);
+        case RETROUVE:
+            robot.setSpeed(100,300);
             break;
             
         case GO_STRAIGHT:
-            robot.setSpeed(80,400);
+            robot.setSpeed(320,1600);
+            break;
+    }
+}
+
+bool automate_fin_de_partie(Robot& robot)
+{
+    typedef enum{AVANCE, EXPLOSE_BALLON, FIN_PARTIE} type_etat;
+    static type_etat etat = AVANCE;
+    
+    static Timer T_fin;
+    
+    switch(etat)
+    {
+        case AVANCE:
+            robot.stopRouleau();
+            if(robot.GoToXYT(2000,3800,0))
+                etat = EXPLOSE_BALLON;
+            break;
+            
+        case EXPLOSE_BALLON:
+            if(robot.leveBras())
+            {
+                robot.exploseBallon();
+                T_fin.start();
+                etat = FIN_PARTIE;
+            }
+            break;
+            
+        case FIN_PARTIE:
+            if(T_fin.read() >= 2.0f && robot.baisseBras())
+            {
+                robot.arreteDisquette();
+                return true;
+            }           
+    }
+    
+    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;
     }
 }
\ No newline at end of file