Diff: main.cpp

Revision:

18:a7dc3a63d7eb

Parent:

15:3d4543a6c100

--- a/main.cpp	Mon Jun 03 16:37:37 2019 +0000
+++ b/main.cpp	Wed Jun 05 12:32:47 2019 +0000
@@ -2,51 +2,98 @@
 #include "CAN_asser.h"
 #include "Robot.h"
 
-void testLasers(Robot&);
+void testLasers(Robot&, bool&);
 void automate_testABalle(Robot&);
-void automate_deplacement(Robot&);
+void automate_deplacement(Robot&, bool&);
 
 
-typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT} type_etat_deplacement;
+typedef enum{TURN_FAST,RETROUVE,GO_STRAIGHT,CORRECT_GAUCHE,CORRECT_DROITE} type_etat_deplacement;
 type_etat_deplacement etat_deplacement = TURN_FAST;
 
 
 int main(void)
 {
     Robot robot;
+    bool autorCorrect = false;
+    DigitalIn Jack(PA_15);
     
-    robot.setSpeed(60,300);
+    robot.setSpeed(100,600);
+    robot.gobe(25);
     
     while(1)
+    if(!Jack)
     {            
         automate_testABalle(robot);
         
-        testLasers(robot);
+        testLasers(robot, autorCorrect);
         
         if(!robot.aBalle())
-            automate_deplacement(robot);
+            automate_deplacement(robot, autorCorrect);
     }
 }
 
-void testLasers(Robot& robot)
-{    
+void testLasers(Robot& robot, bool& autorCorrect)
+{   
+    const int diffMurBalle = 20;
     static float past_gauche, past_droit, actual_gauche, actual_droit;
     
     actual_gauche = robot.getDist(Laser::Gauche);
     actual_droit = robot.getDist(Laser::Droit);
     
-    
-    if(actual_droit < past_droit-30 && etat_deplacement == TURN_FAST)
+    switch(etat_deplacement)
     {
-        robot.stop();
-        etat_deplacement = RETROUVE;
-        robot.setSpeed(30,300);
-    }
-    else if(actual_gauche < past_gauche-30 && etat_deplacement == RETROUVE)
-    {
-        robot.stop();
-        etat_deplacement = GO_STRAIGHT;
-        robot.setSpeed(40,300);
+        case TURN_FAST:
+            if(actual_droit < past_droit - diffMurBalle)
+            {
+                robot.stop();
+                etat_deplacement = RETROUVE;
+                robot.setSpeed(50,300);
+            }
+            break;
+            
+        case RETROUVE:
+            if(actual_droit < past_droit - diffMurBalle)
+            {
+                robot.stop();
+                etat_deplacement = GO_STRAIGHT;
+                robot.setSpeed(100,500);
+            }
+            break;
+            
+        /*case GO_STRAIGHT:
+            if(actual_droit > past_droit + diffMurBalle)
+            {
+                etat_deplacement = CORRECT_GAUCHE;
+                robot.setSpeed(50,300);
+            }
+            else if(actual_gauche > past_gauche + diffMurBalle)
+            {
+                etat_deplacement = CORRECT_DROITE;
+                robot.setSpeed(50,300);
+            }
+            break;*/
+        
+        /*case CORRECT_GAUCHE:
+            if(autorCorrect) //s'il a fini d'avancer
+                if(actual_droit < past_droit - diffMurBalle)
+                {
+                    robot.stop();
+                    autorCorrect = false;
+                    etat_deplacement = GO_STRAIGHT;
+                    robot.setSpeed(80,300);
+                }
+            break;
+            
+        case CORRECT_DROITE:
+            if(autorCorrect) //s'il a fini d'avancer
+                if(actual_gauche < past_gauche - diffMurBalle)
+                {
+                    robot.stop();
+                    autorCorrect = false;
+                    etat_deplacement = GO_STRAIGHT;
+                    robot.setSpeed(80,300);
+                }
+            break;*/
     }
     
     
@@ -59,31 +106,55 @@
     typedef enum {ATTENTE,ABALLE} type_etat;
     static type_etat etat = ATTENTE;
     
+    PwmOut Servo_Ballon(PA_10);
+    PwmOut Mot_Ballon(PB_6);
+    
+    Servo_Ballon.period_ms(20);
+    Mot_Ballon.period_ms(20);
+    
+    static Timer T_ejecte;
+    
     switch(etat)
     {
         case ATTENTE:
-            robot.gobe(25);
             if(robot.aBalle())
             {
                 dbug.printf("A balle\n\r");
                 robot.stop();
+                //Servo_Ballon.write(1);
+                robot.setSpeed(80,500);
                 etat = ABALLE;
             }
+            else if( T_ejecte.read() >= 1.0f )
+            {
+                robot.gobe(25);
+                //Reset du timer pour la prochaine balle
+                T_ejecte.stop();
+                T_ejecte.reset();
+            }
             break;
             
         case ABALLE:
-            if(robot.tourne(1800))
+            /*if(robot.tourne(1800))
             {
-                dbug.printf("Tourne\n\r");
-                robot.ejecte(60);
+                dbug.printf("Tourne\n\r");*/
+                robot.ejecte(70);
+            //}
+            if(!robot.aBalle())
+            {
+                //Timer qui permet à la balle d'avoir le temps de quitter le reservoir
+                T_ejecte.start();
+                //
+                Servo_Ballon.write(0);
                 etat = ATTENTE;
                 etat_deplacement = TURN_FAST;
-            }
+                robot.setSpeed(100,600);
+            }   
             break;
     }
 }
 
-void automate_deplacement(Robot& robot)
+void automate_deplacement(Robot& robot, bool& autorCorrect)
 {    
     switch(etat_deplacement)
     {
@@ -97,7 +168,16 @@
             
         case GO_STRAIGHT:
             robot.avance();
+                //autorCorrect = true;
             break;
+            
+        /*case CORRECT_GAUCHE:
+            robot.tourne(-30);
+            break;
+            
+        case CORRECT_DROITE:
+            robot.tourne(30);
+            break;*/
     }
 }
 
@@ -258,7 +338,7 @@
             
         case BALLE_STRAIGHT:
             robot.avance();
-            /*static float allerJusqua;
+            static float allerJusqua;
             
             if(robot.getDist(Laser::Gauche,"mm") < robot.getDist(Laser::Droit,"mm"))
                 allerJusqua = 0.95*robot.getDist(Laser::Gauche,"mm");
@@ -271,7 +351,7 @@
             while(!robot.avance((int)allerJusqua))
             {
                 dbug.printf("%f\n\r", robot.getDist(Laser::Gauche,"mm"));
-            }*/
-            /*break;
+            }
+            break;
     }
 }*/
\ No newline at end of file