diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 4b30cdc..267e1f2 100644 --- a/Asser_Moteurs.c +++ b/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){ diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h index 704706f..aad202f 100644 --- a/Asser_Moteurs.h +++ b/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); diff --git a/Evitement.c b/Evitement.c index bb880b8..39d0837 100644 --- a/Evitement.c +++ b/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: - if(Trajet_get_bloque() == 1){ + case OBSTACLE_CONFIRME: + if(!Trajet_get_bloque()){ evitement_statu = PAS_D_OBSTACLE; } break; diff --git a/Evitement.h b/Evitement.h index ad42bd6..33fd1f3 100644 --- a/Evitement.h +++ b/Evitement.h @@ -1,5 +1,8 @@ enum evitement_statu_t{ PAS_D_OBSTACLE, OBSTACLE_NON_CONFIRME, - ARRET_DEVANT_OBSTACLE, -}; \ No newline at end of file + OBSTACLE_CONFIRME, +}; + +enum evitement_statu_t Evitement_get_statu(); +void Evitement_gestion(int step_ms); \ No newline at end of file diff --git a/Holonome2023.c b/Holonome2023.c index abdc460..af9ef40 100644 --- a/Holonome2023.c +++ b/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){ diff --git a/Strategie.h b/Strategie.h index 3393595..3678a6b 100644 --- a/Strategie.h +++ b/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; diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index d81508d..83130d4 100644 --- a/Strategie_deplacement.c +++ b/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; } diff --git a/Test_strategie.c b/Test_strategie.c index 487623c..028cdc3 100644 --- a/Test_strategie.c +++ b/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); diff --git a/Trajet.h b/Trajet.h index ac03b00..b93d1ff 100644 --- a/Trajet.h +++ b/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();