Diff: main.cpp

Revision:

27:e103da412e2b

Parent:

26:fa8a8fa175cb

Child:

28:82571fd665bf

diff -r fa8a8fa175cb -r e103da412e2b main.cpp
--- a/main.cpp	Thu Jun 06 13:22:25 2019 +0000
+++ b/main.cpp	Thu Jun 06 14:22:58 2019 +0000
@@ -2,93 +2,151 @@
 #include "CAN_asser.h"
 #include "Robot.h"
 
-void automate_testDeplacement(Robot&);
-bool automate_arretUrgence(Robot&);
+void automate_testLasers(Robot&);
+void automate_testABalle(Robot&);
+void automate_deplacement(Robot&);
+void automate_vitesse(Robot&);
+
+typedef enum{TURN_FAST, CORRECT_GAUCHE, CORRECT_DROITE, GO_STRAIGHT} type_etat_deplacement;
+type_etat_deplacement etat_deplacement = TURN_FAST;
+
 
 int main(void)
 {
     Robot robot;
-
-    robot.setSpeed(80,400);
     
-    Timer timer_dbug;
-    timer_dbug.start();
+    robot.setSpeed(80,450); // vitesse de rotation de recherche de la balle
     
     while(1)
     if(!Robot::Jack)
     {
-        automate_testDeplacement(robot);
+        automate_testABalle(robot);
+        
+        automate_testLasers(robot);
         
-        if(timer_dbug.read() > 0.5f)
-        {
-            timer_dbug.stop();
-            timer_dbug.reset();
-            timer_dbug.start();
-        }
+        if(!robot.aBalle())
+            automate_deplacement(robot);
+            
+        automate_vitesse(robot);
     }
 }
 
-void automate_testDeplacement(Robot& robot)
+
+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)
+    {
+        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;
+    }
+    
+    
+    past_gauche = actual_gauche;
+    past_droit = actual_droit;
+}
+
+void automate_testABalle(Robot& robot)
 {
-    typedef enum{ AVANCE, ARRET_URGENCE } type_etat;
-    static type_etat etat = AVANCE;
+    typedef enum {ATTENTE,ABALLE} type_etat;
+    static type_etat etat = ATTENTE;
     
+    static Timer T_ejecte;
     
     switch(etat)
     {
-        case AVANCE:
-            robot.avance();
-            if( automate_arretUrgence(robot) )
-                etat = ARRET_URGENCE;
+        case ATTENTE:
+            if(robot.aBalle())
+            {
+                robot.stop();
+                robot.exploseBallon();
+                etat = ABALLE;
+                robot.setSpeed(80,500); //vitesse de rotation vers le terrain ennemi
+            }
+            else if( T_ejecte.read() >= 1.0f )
+            {
+                robot.gobe(25);
+                T_ejecte.stop();
+                T_ejecte.reset();
+            }
             break;
             
-        case ARRET_URGENCE:
-            robot.stop();
+        case ABALLE:
+            if(robot.tourne(1800))
+                robot.ejecte(70);
+                
+            if(!robot.aBalle())
+            {
+                robot.arreteDisquette();
+                T_ejecte.start();
+                etat = ATTENTE;
+                etat_deplacement = TURN_FAST;
+            }   
             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)
+void automate_deplacement(Robot& robot)
+{    
+    switch(etat_deplacement)
     {
-        case RAS :
-            if(robot.surBlanc(Robot::CNY_GAUCHE) && robot.surBlanc(Robot::CNY_DROIT)){
-                etat = PERCEPTION;
-                timerCNY.start();
-            }
+        case TURN_FAST:
+            robot.tourne();
+            break;
+            
+        case CORRECT_GAUCHE:
+            robot.tourne(-450);
             break;
             
-        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();
-            }
+        case CORRECT_DROITE:
+            robot.tourne(450);
+            
+        case GO_STRAIGHT:
+            robot.avance();
+            break;
+    }
+}
+
+void automate_vitesse(Robot& robot)
+{
+    switch(etat_deplacement)
+    {
+        case TURN_FAST:
+            robot.setSpeed(80,450);
             break;
             
-        case ARRET_URGENCE :
-            timerCNY.stop();
-            timerCNY.reset();
-            
-            etat = ATTENTE_REPLACEMENT;
+        case CORRECT_GAUCHE:
+            robot.setSpeed(50,300);
+            break;
             
-            return true;
+        case CORRECT_DROITE:
+            robot.setSpeed(50,300);
+            break;
             
-        case ATTENTE_REPLACEMENT :
-            if(!robot.surBlanc(Robot::CNY_GAUCHE) && !robot.surBlanc(Robot::CNY_DROIT))
-                etat = RAS;
+        case GO_STRAIGHT:
+            robot.setSpeed(80,400);
             break;
     }
-    
-    return false;
-}
-            
\ No newline at end of file
+}
\ No newline at end of file