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