Diff: main.cpp

Revision:

23:bb1535360a98

Parent:

20:a1b5d032b422

Child:

24:314b1f6607c5

--- a/main.cpp	Wed Jun 05 16:06:01 2019 +0000
+++ b/main.cpp	Thu Jun 06 11:43:01 2019 +0000
@@ -2,52 +2,146 @@
 #include "CAN_asser.h"
 #include "Robot.h"
 
-void automate_testDeplacement(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(40,400);
     
-    Timer timer_dbug;
-    timer_dbug.start();
+    robot.setSpeed(80,450); // vitesse de rotation de recherche de la balle
+    robot.gobe(25);
     
-    AnalogIn cny_g(A2);
-    AnalogIn cny_d(A3);
+    double pos_servo_ballon = 0.045;
     
     while(1)
+    if(!Robot::Jack)
     {
-        //automate_testDeplacement(robot);
+        if(robot.leveBras())
+            robot.exploseBallon();
+        //automate_testABalle(robot);
         
-        if(timer_dbug.read() > 0.5f)
-        {
-            dbug.printf( "CNY Gauche : %f\n\r", cny_g.read() );
-            dbug.printf( "CNY Droit  : %f\n\r", cny_d.read() );
-            sauter_lignes(20);
-            timer_dbug.stop();
-            timer_dbug.reset();
-            timer_dbug.start();
-        }
+        /*automate_testLasers(robot);
+        
+        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;
+    }
+    
+    
+    past_gauche = actual_gauche;
+    past_droit = actual_droit;
+}
+
+void automate_testABalle(Robot& robot)
 {
-    typedef enum{AVANCE,TOURNE} 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:
-            if(robot.avance(500))
-                etat = TOURNE;
+        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 TOURNE:
-            if(robot.tourne(900))
-                etat = AVANCE;
+        case ABALLE:
+            if(robot.tourne(1800))
+                robot.ejecte(70);
+                
+            if(!robot.aBalle())
+            {
+                robot.arreteDisquette();
+                T_ejecte.start();
+                etat = ATTENTE;
+                etat_deplacement = TURN_FAST;
+            }   
             break;
     }
 }
-            
\ No newline at end of file
+
+void automate_deplacement(Robot& robot)
+{    
+    switch(etat_deplacement)
+    {
+        case TURN_FAST:
+            robot.tourne();
+            break;
+            
+        case CORRECT_GAUCHE:
+            robot.tourne(-450);
+            break;
+            
+        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 CORRECT_GAUCHE:
+            robot.setSpeed(50,300);
+            break;
+            
+        case CORRECT_DROITE:
+            robot.setSpeed(50,300);
+            break;
+            
+        case GO_STRAIGHT:
+            robot.setSpeed(80,400);
+            break;
+    }
+}
\ No newline at end of file