From d7e13c8576bfe72a4712c2c670727adb99573ff8 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Mon, 26 May 2025 20:51:08 +0200
Subject: [PATCH] =?UTF-8?q?Gestion=20de=20l'arr=C3=AAt=20sur=20obstable=20?=
 =?UTF-8?q?fonctionnel=20-=20angle=20plus=20souple?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Cerveau/Cerveau.ino | 66 ++++++++++++++++++++++++++++++---------------
 1 file changed, 45 insertions(+), 21 deletions(-)

diff --git a/Cerveau/Cerveau.ino b/Cerveau/Cerveau.ino
index ed76ad2..026e183 100644
--- a/Cerveau/Cerveau.ino
+++ b/Cerveau/Cerveau.ino
@@ -835,8 +835,47 @@ int detection_adversaire(float angle_deplacement){
   return 0;
 }
 
+#define SEUIL_OBSTACLE_CM 50
+
+/// @brief A partir des déplacements X et Y, selection des capteurs à utiliser pour détecter l'adversaire
+/// @return 1 obstable, 0 pas d'obstacle
+int presence_obstacle(int distance_x_mm, int distance_y_mm){
+  struct detect_adv_reception_t detect_adv_reception;
+  Detect_adv_lire(&detect_adv_reception);
+  float angle = atan2f(distance_y_mm, distance_x_mm);
+  if(angle < 0){
+    angle += 2 * M_PI;
+  }
+  // Angle entre 0 et 2 PI
+  int segment_central = 23 - angle / 2. / M_PI * 24;
+  // segment_central entre 0 et 23;
+  if(segment_central == 23){
+    segment_central = 0;
+  }
+  // segment_central entre 0 et 22;
+  int capteur_central =  segment_central / 2;
+  int capteur_suivant = capteur_central + 1;
+  int capteur_precedent = capteur_central - 1;
+
+  if(capteur_suivant > 11){
+    capteur_suivant = 0;
+  }
+  if(capteur_precedent < 0){
+    capteur_precedent = 11;
+  }
+
+  if(detect_adv_reception.distance_cm[capteur_precedent] < SEUIL_OBSTACLE_CM || 
+      detect_adv_reception.distance_cm[capteur_central] < SEUIL_OBSTACLE_CM || 
+      detect_adv_reception.distance_cm[capteur_suivant] < SEUIL_OBSTACLE_CM ){
+    // Arrêt du mouvement
+    return 1;
+  }
+  return 0;
+
+}
+
 /// @brief Deplacement dans le repère du robot, pouvant prendre en compte la detection de l'adversaire
-/// evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
+/// @param evitement : 1 pour s'arreter si adversaire detecté, 0 pour ignorer l'adversaire
 enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, float rotation_rad, int evitement){
   static enum{
     DR_INIT,
@@ -845,7 +884,6 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
 
   struct chassis_emission_t chassis_emission;
   struct chassis_reception_t chassis_reception;
-  struct detect_adv_reception_t detect_adv_reception;
 
   switch(etat_deplacement){
     case DR_INIT:
@@ -863,25 +901,11 @@ enum etat_action_t deplacement_relatif(int distance_x_mm, int distance_y_mm, flo
 
     case DR_MVT_EN_COUR:
       if(evitement){
-        // On lit les capteurs
-        Detect_adv_lire(&detect_adv_reception);
-        // On analyse les valeurs - TODO : créer une fonction plus évoluée
-        if(distance_x_mm > 0){
-          if(detect_adv_reception.distance_cm[0] < 50){
-            // Arrêt du mouvement
-            chassis_emission.status = MOUVEMENT_INTERRUPTION;
-            send_Chassis(&chassis_emission);
-            etat_deplacement = DR_INIT;
-            return ACTION_ECHEC;
-          }
-        }else if(distance_x_mm < 0){
-          if(detect_adv_reception.distance_cm[6] < 50){
-            // Arrêt du mouvement
-            chassis_emission.status = MOUVEMENT_INTERRUPTION;
-            send_Chassis(&chassis_emission);
-            etat_deplacement = DR_INIT;
-            return ACTION_ECHEC;
-          }
+        if(presence_obstacle(distance_x_mm, distance_y_mm)){
+          chassis_emission.status = MOUVEMENT_INTERRUPTION;
+          send_Chassis(&chassis_emission);
+          etat_deplacement = DR_INIT;
+          return ACTION_ECHEC;
         }
       }