Fonction d'évitement en aller-retour testé

Asser_Moteurs.c

@@ -60,6 +60,19 @@ float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
     return distance / temps;
 }
 
+/// @brief Indique si le robot est à l'arrêt
+/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses)
+/// @return 1 si le robot est immobile, 0 s'il est en mouvement.
+uint32_t AsserMoteur_RobotImmobile(int step_ms){
+    const float seuil_vitesse_immobile_mm_s = 0.1;
+    if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s &&
+            AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s &&
+            AsserMoteur_getVitesse_mm_s(MOTEUR_C, step_ms) < seuil_vitesse_immobile_mm_s ){
+        return 1;
+    }
+    return 0;
+}
+
 /// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
 /// @param step_ms 
 void AsserMoteur_Gestion(int step_ms){

Asser_Moteurs.h

@@ -1,5 +1,6 @@
 #include "Moteurs.h"
 
+uint32_t AsserMoteur_RobotImmobile(int step_ms);
 void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
 float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
 float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);

Evitement.c

@@ -1,4 +1,5 @@
 #include "pico/stdlib.h"
+#include "Asser_Moteurs.h"
 #include "Evitement.h"
 #include "Trajet.h"
 
@@ -7,25 +8,25 @@
 
 enum evitement_statu_t evitement_statu=PAS_D_OBSTACLE;
 
-void Evitement_gestion(){
+void Evitement_gestion(int step_ms){
     static uint32_t temps_obstacle;
     switch(evitement_statu){
         case PAS_D_OBSTACLE:
-            if(Trajet_get_bloque() == 1){
+            if(Trajet_get_bloque() == 1 && AsserMoteur_RobotImmobile(step_ms)){
                 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;
+                evitement_statu = OBSTACLE_CONFIRME;
             }
-            if(Trajet_get_bloque() == 1){
+            if(!Trajet_get_bloque()){
                 evitement_statu = PAS_D_OBSTACLE;
             }
             break;
-        case ARRET_DEVANT_OBSTACLE:
+        case OBSTACLE_CONFIRME:
-            if(Trajet_get_bloque() == 1){
+            if(!Trajet_get_bloque()){
                 evitement_statu = PAS_D_OBSTACLE;
             }
             break;

Evitement.h

@@ -1,5 +1,8 @@
 enum evitement_statu_t{
     PAS_D_OBSTACLE,
     OBSTACLE_NON_CONFIRME,
-    ARRET_DEVANT_OBSTACLE,
+    OBSTACLE_CONFIRME,
 };
+
+enum evitement_statu_t Evitement_get_statu();
+void Evitement_gestion(int step_ms);

Holonome2023.c

@@ -13,6 +13,7 @@
 #include "Asser_Position.h"
 #include "Balise_VL53L1X.h"
 #include "Commande_vitesse.h"
+#include "Evitement.h"
 #include "i2c_annexe.h"
 #include "i2c_maitre.h"
 #include "Localisation.h"
@@ -122,6 +123,7 @@ int main() {
             QEI_update();
             Localisation_gestion();
             AsserMoteur_Gestion(_step_ms);
+            Evitement_gestion(_step_ms);
 
             // Routine à 2 ms
             if(temps_ms % _step_ms_gyro == 0){

Strategie.h

@@ -44,6 +44,14 @@ extern enum etat_homologation_t{
     
 }etat_homologation;
 
+enum evitement_t{
+    SANS_EVITEMENT,
+    PAUSE_DEVANT_OBSTACLE,
+    ARRET_DEVANT_OBSTACLE,
+    RETOUR_SI_OBSTABLE,
+    CONTOURNEMENT
+};
+
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
 enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
@@ -58,6 +66,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
 
 int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
 enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement);
 
 extern float distance_obstacle;
 

Strategie_deplacement.c

@@ -4,7 +4,62 @@
 #include "Geometrie.h"
 #include "Balise_VL53L1X.h"
 
-enum etat_action_t Strategie_parcourir_trajet(){
+enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, uint32_t step_ms, enum evitement_t evitement){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_trajet_t etat_trajet;
+    float angle_avancement;
+    
+    static enum {
+        PARCOURS_INIT,
+        PARCOURS_AVANCE,
+    } etat_parcourt=PARCOURS_INIT;
+
+    switch (etat_parcourt){
+        case PARCOURS_INIT:
+            Trajet_debut_trajectoire(trajectoire);
+            etat_parcourt = PARCOURS_AVANCE;
+            break;
+        
+        case PARCOURS_AVANCE:
+            if(evitement != SANS_EVITEMENT){
+                angle_avancement = Trajet_get_orientation_avance();
+                distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement);
+                Trajet_set_obstacle_mm(distance_obstacle);
+            }
+
+            if(Evitement_get_statu() == ARRET_DEVANT_OBSTACLE){
+                switch(evitement){
+                    case SANS_EVITEMENT:
+                        //printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n");
+                        break;
+                    case PAUSE_DEVANT_OBSTACLE:
+                        // Rien à faire ici
+                        break;
+                    
+                    case ARRET_DEVANT_OBSTACLE:
+                        etat_parcourt = PARCOURS_INIT;
+                        return ACTION_TERMINEE;
+
+                    case RETOUR_SI_OBSTABLE:
+                        Trajet_inverse();
+                        break;
+
+                    case CONTOURNEMENT: // TODO
+                        break;
+                        
+                }
+
+            }
+
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                etat_action = ACTION_TERMINEE;
+                etat_parcourt = PARCOURS_INIT;
+            }
+            break;
+    }
+
+    return etat_action;
 
 }
 

Test_strategie.c

@@ -5,6 +5,7 @@
 
 #include "Asser_Moteurs.h"
 #include "Balise_VL53L1X.h"
+#include "Evitement.h"
 #include "i2c_annexe.h"
 #include "i2c_maitre.h"
 #include "gyro.h"
@@ -297,8 +298,31 @@ void affichage_test_evitement(){
 int test_evitement(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
     struct trajectoire_t trajectoire;
+    enum evitement_t evitement;
+    enum etat_action_t etat_action;
     printf("Evitement\n");
 
+    printf("A - Sans evitement.\n");
+    printf("B - Pause devant obstacle.\n");
+    printf("C - Arrêt devant obstacle.\n");
+    printf("D - Retour si obstacle.\n");
+    printf("E - Contournement.\n");
+    do{
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+    switch(lettre){
+        case 'a':
+        case 'A':evitement=SANS_EVITEMENT;break;
+        case 'b':
+        case 'B':evitement=PAUSE_DEVANT_OBSTACLE;break;
+        case 'c':
+        case 'C':evitement=ARRET_DEVANT_OBSTACLE;break;
+        case 'd':
+        case 'D':evitement=RETOUR_SI_OBSTABLE;break;
+        case 'e':
+        case 'E':evitement=CONTOURNEMENT;break;
+    }
+
     i2c_maitre_init();
     Trajet_init();
     Balise_VL53L1X_init();
@@ -327,6 +351,7 @@ int test_evitement(){
             QEI_update();
             Localisation_gestion();
             AsserMoteur_Gestion(_step_ms);
+            Evitement_gestion(_step_ms);
 
             // Routine à 2 ms
             if(temps_ms % _step_ms_gyro == 0){
@@ -339,14 +364,12 @@ int test_evitement(){
                                 1000,200,
                                 0, 0); // Angles
 
-            if(parcourt_trajet_simple(trajectoire, _step_ms) == ACTION_TERMINEE){
+            etat_action = Strategie_parcourir_trajet(trajectoire, _step_ms, evitement);
-
-            }
 
         }
         lettre = getchar_timeout_us(0);
     //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
-    }while(1);
+    }while(etat_action == ACTION_EN_COURS);
     printf("STRATEGIE_LOOP_2\n");
     printf("Lettre : %d; %c\n", lettre, lettre);
     

Trajet.h

@@ -26,3 +26,4 @@ void Trajet_stop(float);
 float Trajet_get_orientation_avance(void);
 float Trajet_get_abscisse();
 uint32_t Trajet_get_bloque();
+void Trajet_inverse();