Diff: main.cpp

Revision:

12:7ec2c5b674a2

Parent:

11:e8c4a1c6553d

--- a/main.cpp	Mon May 27 17:42:53 2019 +0000
+++ b/main.cpp	Tue May 28 06:50:44 2019 +0000
@@ -17,11 +17,31 @@
 {
     Robot robot;
     
-    robot.gobe(40);
+    int pos[3];
+    
+    //Pour cadencer la fréquence d'envoi de messages vers le module bluetooth
+    Timer timer_dbug;
+    timer_dbug.start();
+    
+    robot.gobe(30);
     
     while(1)
     {
+        //Maj de la position du robot
+        robot.getPos(pos);
+        //Led s'allume sur la carte si le robot a une balle
         robot.LedBps = robot.aBalle();
+        
+        if(timer_dbug.read() > 1.f){
+        dbug.printf("posX  : %d\n\r", pos[0]);
+        dbug.printf("posY  : %d\n\r", pos[1]);
+        dbug.printf("Angle : %d\n\r", pos[2]);
+        sauter_lignes(24);
+        
+        timer_dbug.stop();  timer_dbug.reset();  timer_debug.start();
+    }
+        
+        //Automate
         automate_test(robot);
     }
 }
@@ -29,43 +49,30 @@
 
 void automate_test(Robot& robot)
 {
-    typedef enum {DEBUT, ALLER_ENDROIT, EJECTE, RETOUR_DEBUT} type_etat;
-    static type_etat etat = DEBUT;
-    
-    int pos[3];
-    robot.getPos(pos);
-    
-    static Timer timer_dbug;
-    timer_dbug.start();
-    if(timer_dbug.read() > 1.f){
-        dbug.printf("posX  : %d\n\r", pos[0]);
-        dbug.printf("posY  : %d\n\r", pos[1]);
-        dbug.printf("Angle : %d\n\r", pos[2]);
-        sauter_lignes(24);
-        timer_dbug.start();
-    } 
+    typedef enum {DEBUT, ATTENTE_BALLE, TOURNE, EJECTE} type_etat;
+    static type_etat etat = DEBUT; 
     
     switch(etat)
     {
         case DEBUT:
+            robot.avance(300);
+            etat = ATTENTE_BALLE;
+            break;
+            
+        case ATTENTE_BALLE:
             if(robot.aBalle()){
-                etat = ALLER_ENDROIT;
-                robot.GoToXYT(500, 500, 900);
+                SendRawId(ASSERVISSEMENT_STOP);
+                etat = TOURNE;
             }
             break;
             
-        case ALLER_ENDROIT:
-            if(val_abs(pos[0]-500)<10 && val_abs(pos[1]-500)<10 && val_abs(pos[2]-900)<10)
+        case TOURNE:
+            if(robot.tourne(1800))
                 etat = EJECTE;
             break;
             
         case EJECTE:
-            robot.ejecte(50);
-            etat = RETOUR_DEBUT;
-            break;
-            
-        case RETOUR_DEBUT:
-            robot.GoToXYT(0, 0, 0, 1);
+            robot.ejecte(60);
             etat = DEBUT;
             break;
     }