Diff: main.cpp

Revision:

13:9c62e263f245

Parent:

10:efa507ba2b35

Child:

14:1be2f691cbb4

--- a/main.cpp	Thu May 23 21:26:08 2019 +0000
+++ b/main.cpp	Tue May 28 06:48:54 2019 +0000
@@ -2,15 +2,13 @@
 #include "CAN_asser.h"
 #include "Robot.h"
 
-/*//Bonus fin de partie
-PwmOut bonus(PA_10);
-
-//Disquette
-PwmOut Servo_Ballon(PB_7);
-PwmOut Mot_Ballon(PB_6);*/
+void automate_testLasers(Robot&);
+void automate_testABalle(Robot&);
+void automate_deplacement(Robot&);
 
 
-void automate_test(Robot& R);
+typedef enum{CHERCHE_BALLE,BALLE_GAUCHE,BALLE_DROITE,BALLE_STRAIGHT} type_etat_balle;
+type_etat_balle etat_balle = CHERCHE_BALLE;
 
 
 int main(void)
@@ -18,25 +16,142 @@
     Robot robot;
     
     while(1)
-        automate_test(robot);
+    {
+        automate_testLasers(robot);
+        automate_testABalle(robot);
+        if(!robot.aBalle())
+            automate_deplacement(robot);
+    }
 }
 
 
-void automate_test(Robot& R)
+void automate_testLasers(Robot& robot)
 {
-    typedef enum {AVANCE, TOURNE} type_etat;
-    static type_etat etat = AVANCE;
+    typedef enum {ATTENTE, TURN_RIGHT, TURN_LEFT, VOIT_BALLE} type_etat;
+    static type_etat etat = ATTENTE;
+    
+    static float past_gauche, past_droit, actual_gauche, actual_droit;
+    
+    actual_gauche = robot.getDist(Laser::Gauche);
+    actual_droit = robot.getDist(Laser::Droit);
     
     switch(etat)
     {
-        case AVANCE:
-            if(R.avance(500))
-                etat = TOURNE;
+        case ATTENTE:            
+            if(actual_gauche < past_gauche-10)
+            {
+                etat = TURN_LEFT;
+                robot.stop();
+            }
+                
+            if(actual_droit < past_droit-10)
+            {
+                etat = TURN_RIGHT;
+                robot.stop();
+            }
+            break;
+            
+            
+        case TURN_LEFT:
+            etat_balle = BALLE_GAUCHE;
+            
+            if(actual_droit < past_droit-10)
+            {
+                etat = VOIT_BALLE;
+                robot.stop();
+            }
+                
+            if(actual_gauche > past_gauche+10)
+            {
+                etat = TURN_RIGHT;
+                robot.stop();
+            }
+            break;
+            
+            
+        case TURN_RIGHT:
+            etat_balle = BALLE_DROITE;
+            
+            if(actual_gauche < past_gauche-10)
+            {
+                etat = VOIT_BALLE;
+                robot.stop();
+            }
+                
+            if(actual_droit > past_droit+10)
+            {
+                etat = TURN_LEFT;
+                robot.stop();
+            }
             break;
             
-        case TOURNE:
-            if(R.tourne(900))
-                etat = AVANCE;
+            
+        case VOIT_BALLE:
+            etat_balle = BALLE_STRAIGHT;
+                
+            if(actual_gauche > past_gauche+10)
+            {
+                etat = TURN_RIGHT;
+                robot.stop();
+            }
+                
+            if(actual_droit > past_droit+10)
+            {
+                etat = TURN_LEFT;
+                robot.stop();
+            }
+            break;
+    }
+    
+    past_gauche = actual_gauche;
+    past_droit = actual_droit;
+}
+
+void automate_testABalle(Robot& robot)
+{
+    typedef enum {ATTENTE,ABALLE} type_etat;
+    static type_etat etat = ATTENTE;
+    
+    switch(etat)
+    {
+        case ATTENTE:
+            robot.gobe(40);
+            if(robot.aBalle())
+            {
+                robot.stop();
+                etat = ABALLE;
+                etat_balle = CHERCHE_BALLE;
+            }
+            break;
+            
+        case ABALLE:
+            if(robot.tourne(1800))
+            {
+                robot.ejecte(60);
+                etat = ATTENTE;
+            }
+            break;
+    }
+}
+
+void automate_deplacement(Robot& robot)
+{    
+    switch(etat_balle)
+    {
+        case CHERCHE_BALLE:
+            robot.tourne(3600);
+            break;
+            
+        case BALLE_GAUCHE:
+            robot.tourne(-50);
+            break;
+            
+        case BALLE_DROITE:
+            robot.tourne(50);
+            break;
+            
+        case BALLE_STRAIGHT:
+            robot.avance(100);
             break;
     }
 }
\ No newline at end of file