Creation de la fonction gerant l'état de l'évitement

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

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;
+}

Evitement.h

@@ -0,0 +1,5 @@
+enum evitement_statu_t{
+    PAS_D_OBSTACLE,
+    OBSTACLE_NON_CONFIRME,
+    ARRET_DEVANT_OBSTACLE,
+};

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;
 }

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();