Diff: main.cpp

Revision:

22:f18eee9c8714

Parent:

20:a1b5d032b422

--- a/main.cpp	Wed Jun 05 16:06:01 2019 +0000
+++ b/main.cpp	Wed Jun 05 17:24:38 2019 +0000
@@ -3,28 +3,23 @@
 #include "Robot.h"
 
 void automate_testDeplacement(Robot&);
+bool automate_arretUrgence(Robot&);
 
 int main(void)
 {
     Robot robot;
 
-    robot.setSpeed(40,400);
+    robot.setSpeed(80,400);
     
     Timer timer_dbug;
     timer_dbug.start();
     
-    AnalogIn cny_g(A2);
-    AnalogIn cny_d(A3);
-    
     while(1)
     {
-        //automate_testDeplacement(robot);
+        automate_testDeplacement(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();
@@ -34,20 +29,65 @@
 
 void automate_testDeplacement(Robot& robot)
 {
-    typedef enum{AVANCE,TOURNE} type_etat;
+    typedef enum{ AVANCE, ARRET_URGENCE } type_etat;
     static type_etat etat = AVANCE;
     
+    
     switch(etat)
     {
         case AVANCE:
-            if(robot.avance(500))
-                etat = TOURNE;
+            robot.avance(2000);
+            if( automate_arretUrgence(robot) )
+                etat = ARRET_URGENCE;
             break;
             
-        case TOURNE:
-            if(robot.tourne(900))
-                etat = AVANCE;
+        case ARRET_URGENCE:
+            robot.stop();
             break;
     }
 }
+
+bool automate_arretUrgence(Robot& robot)
+{
+    typedef enum{RAS, PERCEPTION, ARRET_URGENCE, ATTENTE_REPLACEMENT} type_etat;
+    static type_etat etat = RAS;
+    
+    // Timer pour la durée sur la ligne blanche
+    static Timer timerCNY;
+    
+    switch(etat)
+    {
+        case RAS :
+            if( robot.surBlanc( Robot::CNY_GAUCHE ) ){
+                etat = PERCEPTION;
+                timerCNY.start();
+            }
+            break;
+            
+        case PERCEPTION :
+            if( robot.surBlanc( Robot::CNY_GAUCHE ) && timerCNY.read() >= 0.25f )
+                etat = ARRET_URGENCE;
+            else if( timerCNY.read() >= 0.25f ){
+                etat = RAS;
+                timerCNY.stop();
+                timerCNY.reset();
+            }
+            break;
+            
+        case ARRET_URGENCE :
+            timerCNY.stop();
+            timerCNY.reset();
+            
+            etat = ATTENTE_REPLACEMENT;
+            
+            return true;
+            
+        case ATTENTE_REPLACEMENT :
+            if( !robot.surBlanc( Robot:: CNY_GAUCHE ) )
+                etat = RAS;
+            break;
+    }
+    
+    return false;
+}
             
\ No newline at end of file