Amélioration de l'algo d'approche des gradins. Il met entre 8 et 20 secondes pour s'approcher d'un gradin

Cerveau/Cerveau.ino

@@ -25,6 +25,9 @@
 #define COULEUR_BLEU 0
 #define COULEUR_JAUNE 1
 
+#define GRADIN_PRECISION_ANGLE_RAD 0.009
+#define GRADIN_PRECISION_X_MM 10
+
 #define gst_server;
 
 extern const char *ssid;
@@ -504,15 +507,19 @@ enum etat_action_t gradin_approche(void){
     GA_CHERCHE_GAUCHE,
     GA_CHERCHE_DROIT,
     GA_GOTO_LARGE,
+    GA_GOTO_EN_FACE,
     GA_TOURNE_LARGE,
     GA_GOTO_PROCHE,
-    GA_GOTO_PREND
+    GA_GOTO_PREND,
+    GA_RECULE
   } statu_approche_gradin = GA_INIT;
   static float angle_parcouru, angle_mem;
   static int nb_erreur;
   int translation_x, translation_y;
+  float distance;
   struct detect_gradin_t detect_gradin;
 
+  delay(150);
   Detect_gradin(&detect_gradin);
   char chaine[200];
   sprintf(chaine, "I2C OK\nStatus:%d\nCentre X:%4d\nCentre Y:%4d\nAngle:%2.2f\n", detect_gradin.status, 
@@ -531,13 +538,18 @@ enum etat_action_t gradin_approche(void){
         statu_approche_gradin = GA_GOTO_LARGE;
         angle_mem = detect_gradin.angle_rad;
         nb_erreur = 0;
+      }else if(detect_gradin.centre_y_mm < 180){
+        statu_approche_gradin = GA_RECULE;
+      }else if(detect_gradin.status == 3){
+        // On a un grain sur la droite
+        statu_approche_gradin = GA_CHERCHE_DROIT;
       }else if(detect_gradin.status == 0){
         // On a perdu la détection
-        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
+        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
         statu_approche_gradin = GA_CHERCHE_DROIT;
       }else{
         // On tourne à gauche de quelques degrés
-        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
+        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
       }
       break;
 
@@ -547,16 +559,22 @@ enum etat_action_t gradin_approche(void){
         statu_approche_gradin = GA_GOTO_LARGE;
         angle_mem = detect_gradin.angle_rad;
         nb_erreur = 0;
+      }else if(detect_gradin.centre_y_mm < 180){
+        statu_approche_gradin = GA_RECULE;
+      }else if(detect_gradin.status == 4){
+        // On a un grain sur la gauche
+        statu_approche_gradin = GA_CHERCHE_GAUCHE;
       }else if(detect_gradin.status == 0){
         // On a perdu la détection
         statu_approche_gradin = GA_INIT;
         return ACTION_ECHEC;
       }else{
         // On tourne à gauche de quelques degrés
-        while(deplacement_relatif(0, 0, -3. * M_PI / 180., 0) == ACTION_EN_COURS);
+        while(deplacement_relatif(0, 0, 3. * M_PI / 180., 0) == ACTION_EN_COURS);
       }
       break;
 
+    case GA_GOTO_EN_FACE:
     case GA_GOTO_LARGE:
       Detect_gradin(&detect_gradin);
       if(detect_gradin.status != 2){
@@ -565,35 +583,69 @@ enum etat_action_t gradin_approche(void){
           affiche_erreur("Gradin Approche", "GA_GOTO_LARGE\n Status != 2");
           while(1);
         }
-      }
-      translation_x = (detect_gradin.centre_y_mm + 150) * tan(detect_gradin.angle_rad);
-      translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
+      }else if(detect_gradin.centre_y_mm < 180){
+        // On est trop près, on ne détectera pas le gradin si on est devant
+        statu_approche_gradin = GA_RECULE;
+      }else{
+        // On se centre par rapport au gradin
+        //translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
+        //translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
+        translation_x = (detect_gradin.centre_y_mm + 150) * tan(fabs(detect_gradin.angle_rad));
+        translation_y = -(detect_gradin.centre_y_mm + 150) * sin(detect_gradin.angle_rad) - detect_gradin.centre_x_mm;
 
-      if(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE){
+        while(deplacement_relatif(translation_x, translation_y, 0, 0) == ACTION_TERMINEE);
         statu_approche_gradin = GA_TOURNE_LARGE;
+        angle_mem = detect_gradin.angle_rad;
+        
       }
       break;
 
     case GA_TOURNE_LARGE:
-      if(deplacement_relatif(0, 0, -angle_mem, 0) == ACTION_TERMINEE){
-        Detect_gradin(&detect_gradin);
-        if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){
-          statu_approche_gradin = GA_CHERCHE_GAUCHE;
-        }else{
+      while(deplacement_relatif(0, 0, -angle_mem, 0) != ACTION_TERMINEE);
+      delay(150);
+      Detect_gradin(&detect_gradin);
+      if(detect_gradin.status != 2){
+        statu_approche_gradin = GA_CHERCHE_GAUCHE;
+      }else if(detect_gradin.centre_y_mm < 180){
+        statu_approche_gradin = GA_RECULE;
+      }else if(fabs(detect_gradin.angle_rad) > GRADIN_PRECISION_ANGLE_RAD){
+        angle_mem = detect_gradin.angle_rad;
+      }else{
+        if(detect_gradin.centre_y_mm > 240){
           statu_approche_gradin = GA_GOTO_PROCHE;
+        }else{
+          if(fabs(detect_gradin.centre_x_mm) > 10){
+            statu_approche_gradin = GA_GOTO_LARGE;
+          }else{
+            statu_approche_gradin = GA_GOTO_PREND;
+          }
         }
-
       }
       break;
 
     case GA_GOTO_PROCHE:
       Detect_gradin(&detect_gradin);
-      if(deplacement_relatif((detect_gradin.centre_y_mm - 20), 0, 0, 0) == ACTION_TERMINEE){
-        statu_approche_gradin = GA_INIT;
-        return ACTION_TERMINEE;
+      if(deplacement_relatif((detect_gradin.centre_y_mm - 200), 0, 0, 0) == ACTION_TERMINEE){
+          statu_approche_gradin = GA_GOTO_EN_FACE;
       }
       break;
 
+    case GA_GOTO_PREND:
+      while(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) != ACTION_TERMINEE);
+      statu_approche_gradin = GA_INIT;
+      delay(5000);
+      return ACTION_TERMINEE;
+  
+      break;
+
+    case GA_RECULE:
+      while(deplacement_relatif((detect_gradin.centre_y_mm - 220), 0, 0, 0) != ACTION_TERMINEE);
+      statu_approche_gradin = GA_CHERCHE_GAUCHE;
+      break;
+
+
+      
+
   }
   return ACTION_EN_COURS;