From 802205220a9e42ffc62dcd7a9c417d9b616ffc1c Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 24 Apr 2025 21:24:40 +0200
Subject: [PATCH] =?UTF-8?q?Am=C3=A9lioration=20de=20l'approche=20des=20gra?=
 =?UTF-8?q?dins,=20une=20=C3=A9tapes=20de=20recalage=20=C3=A0=20180=20mm?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Cerveau/Cerveau.ino | 24 +++++++++++++++++++-----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino
index ed13426..39715b9 100644
--- a/Cerveau/Cerveau.ino
+++ b/Cerveau/Cerveau.ino
@@ -504,6 +504,7 @@ 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
@@ -511,6 +512,7 @@ enum etat_action_t gradin_approche(void){
   static float angle_parcouru, angle_mem;
   static int nb_erreur;
   int translation_x, translation_y;
+  float distance;
   struct detect_gradin_t detect_gradin;
 
   Detect_gradin(&detect_gradin);
@@ -557,6 +559,7 @@ enum etat_action_t gradin_approche(void){
       }
       break;
 
+    case GA_GOTO_EN_FACE:
     case GA_GOTO_LARGE:
       Detect_gradin(&detect_gradin);
       if(detect_gradin.status != 2){
@@ -566,7 +569,7 @@ enum etat_action_t gradin_approche(void){
           while(1);
         }
       }
-      translation_x = (detect_gradin.centre_y_mm + 150) * tan(detect_gradin.angle_rad);
+      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){
@@ -580,7 +583,11 @@ enum etat_action_t gradin_approche(void){
         if(detect_gradin.status != 2 || fabs(detect_gradin.angle_rad) > 0.009){
           statu_approche_gradin = GA_CHERCHE_GAUCHE;
         }else{
-          statu_approche_gradin = GA_GOTO_PROCHE;
+          if(detect_gradin.centre_y_mm > 220){
+            statu_approche_gradin = GA_GOTO_PROCHE;
+          }else{
+            statu_approche_gradin = GA_GOTO_PREND;
+          }
         }
 
       }
@@ -588,12 +595,19 @@ enum etat_action_t gradin_approche(void){
 
     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 - 180), 0, 0, 0) == ACTION_TERMINEE){
+          statu_approche_gradin = GA_GOTO_EN_FACE;
       }
       break;
 
+    case GA_GOTO_PREND:
+      if(deplacement_relatif((detect_gradin.centre_y_mm - 25), 0, 0, 0) == ACTION_TERMINEE){
+          statu_approche_gradin = GA_INIT;
+          return ACTION_TERMINEE;
+      }
+      break;
+      
+
   }
   return ACTION_EN_COURS;