Diff: main.cpp

Revision:

11:e8c4a1c6553d

Parent:

10:efa507ba2b35

Child:

12:7ec2c5b674a2

--- a/main.cpp	Thu May 23 21:26:08 2019 +0000
+++ b/main.cpp	Mon May 27 17:42:53 2019 +0000
@@ -17,26 +17,56 @@
 {
     Robot robot;
     
+    robot.gobe(40);
+    
     while(1)
+    {
+        robot.LedBps = robot.aBalle();
         automate_test(robot);
+    }
 }
 
 
-void automate_test(Robot& R)
+void automate_test(Robot& robot)
 {
-    typedef enum {AVANCE, TOURNE} type_etat;
-    static type_etat etat = AVANCE;
+    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();
+    } 
     
     switch(etat)
     {
-        case AVANCE:
-            if(R.avance(500))
-                etat = TOURNE;
+        case DEBUT:
+            if(robot.aBalle()){
+                etat = ALLER_ENDROIT;
+                robot.GoToXYT(500, 500, 900);
+            }
             break;
             
-        case TOURNE:
-            if(R.tourne(900))
-                etat = AVANCE;
+        case ALLER_ENDROIT:
+            if(val_abs(pos[0]-500)<10 && val_abs(pos[1]-500)<10 && val_abs(pos[2]-900)<10)
+                etat = EJECTE;
+            break;
+            
+        case EJECTE:
+            robot.ejecte(50);
+            etat = RETOUR_DEBUT;
+            break;
+            
+        case RETOUR_DEBUT:
+            robot.GoToXYT(0, 0, 0, 1);
+            etat = DEBUT;
             break;
     }
 }
\ No newline at end of file