From a656d62d4194235842b9c1adc8409b568b821a1e Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Sat, 13 May 2023 20:56:53 +0200
Subject: [PATCH] =?UTF-8?q?Creation=20de=20la=20fonction=20gerant=20l'?=
 =?UTF-8?q?=C3=A9tat=20de=20l'=C3=A9vitement?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CMakeLists.txt |  1 +
 Evitement.c    | 37 +++++++++++++++++++++++++++++++++++++
 Evitement.h    |  5 +++++
 Trajet.c       | 15 ++++++++++++++-
 Trajet.h       |  2 ++
 5 files changed, 59 insertions(+), 1 deletion(-)
 create mode 100644 Evitement.c
 create mode 100644 Evitement.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0f49776..26d7336 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ Asser_Moteurs.c
 Asser_Position.c
 Balise_VL53L1X.c
 Commande_vitesse.c
+Evitement.c
 QEI.c
 Geometrie.c
 gyro.c
diff --git a/Evitement.c b/Evitement.c
new file mode 100644
index 0000000..bb880b8
--- /dev/null
+++ b/Evitement.c
@@ -0,0 +1,37 @@
+#include "pico/stdlib.h"
+#include "Evitement.h"
+#include "Trajet.h"
+
+// 250 ms
+#define TEMPS_VALIDE_OBSTACLE_US 250000
+
+enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
+
+void Evitement_gestion(){
+    static uint32_t temps_obstacle;
+    switch(evitement_statu){
+        case PAS_D_OBSTACLE:
+            if(Trajet_get_bloque() == 1){
+                evitement_statu = OBSTACLE_NON_CONFIRME;
+                temps_obstacle = time_us_32();
+            }
+            break;
+        case OBSTACLE_NON_CONFIRME:
+            if(time_us_32() - temps_obstacle >  TEMPS_VALIDE_OBSTACLE_US){
+                evitement_statu = ARRET_DEVANT_OBSTACLE;
+            }
+            if(Trajet_get_bloque() == 1){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+        case ARRET_DEVANT_OBSTACLE:
+            if(Trajet_get_bloque() == 1){
+                evitement_statu = PAS_D_OBSTACLE;
+            }
+            break;
+    }
+}
+
+enum evitement_statu_t Evitement_get_statu(){
+    return evitement_statu;
+}
\ No newline at end of file
diff --git a/Evitement.h b/Evitement.h
new file mode 100644
index 0000000..ad42bd6
--- /dev/null
+++ b/Evitement.h
@@ -0,0 +1,5 @@
+enum evitement_statu_t{
+    PAS_D_OBSTACLE,
+    OBSTACLE_NON_CONFIRME,
+    ARRET_DEVANT_OBSTACLE,
+};
\ No newline at end of file
diff --git a/Trajet.c b/Trajet.c
index 78750db..e3fa5be 100644
--- a/Trajet.c
+++ b/Trajet.c
@@ -21,6 +21,8 @@ float distance_obstacle_mm;
 float distance_fin_trajectoire_mm;
 const float distance_pas_obstacle = 2000;
 
+float vitesse_max_contrainte_obstacle;
+
 /// @brief Initialise le module Trajet. A appeler en phase d'initilisation
 void Trajet_init(){
     abscisse = 0;
@@ -118,7 +120,6 @@ struct position_t Trajet_get_consigne(){
 /// @return vitesse déterminée en m/s
 float Trajet_calcul_vitesse(float pas_de_temps_s){
     float vitesse_max_contrainte;
-    float vitesse_max_contrainte_obstacle;
     float distance_contrainte,distance_contrainte_obstacle;
     float vitesse;
     // Calcul de la vitesse avec acceleration
@@ -187,4 +188,16 @@ float Trajet_get_orientation_avance(){
 
 float Trajet_get_abscisse(){
     return abscisse;
+}
+
+/// @brief Indique si le robot est bloqué sur le trajet
+/// @return 0 si le robot n'est pas bloqué, 1 s'il est bloqué
+uint32_t Trajet_get_bloque(){
+    if(Trajet_get_obstacle_mm() == DISTANCE_INVALIDE){
+        return 0;
+    }
+    if (vitesse_max_contrainte_obstacle == 0){
+        return 1;
+    }
+    return 0;
 }
\ No newline at end of file
diff --git a/Trajet.h b/Trajet.h
index 53c1a7d..ac03b00 100644
--- a/Trajet.h
+++ b/Trajet.h
@@ -1,3 +1,4 @@
+#include "pico/stdlib.h"
 #include "Trajectoire.h"
 
 enum etat_trajet_t{
@@ -24,3 +25,4 @@ void Trajet_set_obstacle_mm(float distance_mm);
 void Trajet_stop(float);
 float Trajet_get_orientation_avance(void);
 float Trajet_get_abscisse();
+uint32_t Trajet_get_bloque();