From 374da66a5e2c8b7f1ded1843c740efc1780a30ef Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 13 May 2023 22:09:24 +0200 Subject: [PATCH] WIP --- CMakeLists.txt | 1 + Holonome2023.c | 2 +- Strategie.c | 63 +------------------------------------ Strategie_deplacement.c | 70 +++++++++++++++++++++++++++++++++++++++++ Strategie_deplacement.h | 0 Trajet.c | 9 ++++++ 6 files changed, 82 insertions(+), 63 deletions(-) create mode 100644 Strategie_deplacement.c create mode 100644 Strategie_deplacement.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 26d7336..55b31a2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ Robot_config.c Score.c Servomoteur.c Strategie.c +Strategie_deplacement.c Strategie_prise_cerises.c Temps.c Test.c diff --git a/Holonome2023.c b/Holonome2023.c index 2b969fb..abdc460 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -82,7 +82,7 @@ int main() { AsserMoteur_Init(); Localisation_init(); - //while(mode_test()); + while(mode_test()); i2c_maitre_init(); Trajet_init(); Balise_VL53L1X_init(); diff --git a/Strategie.c b/Strategie.c index 50909a2..3b35b1c 100644 --- a/Strategie.c +++ b/Strategie.c @@ -1,7 +1,6 @@ #include "hardware/gpio.h" #include "i2c_annexe.h" #include "Asser_Position.h" -#include "Balise_VL53L1X.h" #include "Commande_vitesse.h" #include "Geometrie_robot.h" #include "Localisation.h" @@ -484,7 +483,7 @@ enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){ if(temporisation_terminee(&tempo_ms, step_ms)){ i2c_annexe_mi_ferme_porte(); etat_lance_balle = LANCE_TEMPO_PROP_ON; - tempo_ms = 750; + tempo_ms = 500; } break; @@ -559,66 +558,6 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float return etat_action; } -enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){ - 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: - angle_avancement = Trajet_get_orientation_avance(); - distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement); - Trajet_set_obstacle_mm(distance_obstacle); - - etat_trajet = Trajet_avance(step_ms/1000.); - if(etat_trajet == TRAJET_TERMINE){ - Trajet_set_obstacle_mm(DISTANCE_INVALIDE); - etat_action = ACTION_TERMINEE; - etat_parcourt = PARCOURS_INIT; - } - break; - } - - return etat_action; -} - -enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){ - enum etat_action_t etat_action = ACTION_EN_COURS; - enum etat_trajet_t etat_trajet; - static enum { - PARCOURS_INIT, - PARCOURS_AVANCE, - } etat_parcourt=PARCOURS_INIT; - - switch (etat_parcourt){ - case PARCOURS_INIT: - Trajet_debut_trajectoire(trajectoire); - Trajet_set_obstacle_mm(distance_pas_obstacle); - etat_parcourt = PARCOURS_AVANCE; - break; - - case PARCOURS_AVANCE: - etat_trajet = Trajet_avance(step_ms/1000.); - if(etat_trajet == TRAJET_TERMINE){ - etat_action = ACTION_TERMINEE; - etat_parcourt = PARCOURS_INIT; - } - break; - } - - return etat_action; -} - /// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette uint attente_tirette(void){ return !gpio_get(TIRETTE); diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c new file mode 100644 index 0000000..d81508d --- /dev/null +++ b/Strategie_deplacement.c @@ -0,0 +1,70 @@ +#include "Strategie.h" +#include "Trajet.h" +#include "Evitement.h" +#include "Geometrie.h" +#include "Balise_VL53L1X.h" + +enum etat_action_t Strategie_parcourir_trajet(){ + +} + + +enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){ + 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: + angle_avancement = Trajet_get_orientation_avance(); + distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement); + Trajet_set_obstacle_mm(distance_obstacle); + + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + Trajet_set_obstacle_mm(DISTANCE_INVALIDE); + etat_action = ACTION_TERMINEE; + etat_parcourt = PARCOURS_INIT; + } + break; + } + + return etat_action; +} + +enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms){ + enum etat_action_t etat_action = ACTION_EN_COURS; + enum etat_trajet_t etat_trajet; + static enum { + PARCOURS_INIT, + PARCOURS_AVANCE, + } etat_parcourt=PARCOURS_INIT; + + switch (etat_parcourt){ + case PARCOURS_INIT: + Trajet_debut_trajectoire(trajectoire); + Trajet_set_obstacle_mm(distance_pas_obstacle); + etat_parcourt = PARCOURS_AVANCE; + break; + + case PARCOURS_AVANCE: + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + etat_action = ACTION_TERMINEE; + etat_parcourt = PARCOURS_INIT; + } + break; + } + + return etat_action; +} \ No newline at end of file diff --git a/Strategie_deplacement.h b/Strategie_deplacement.h new file mode 100644 index 0000000..e69de29 diff --git a/Trajet.c b/Trajet.c index e3fa5be..a719628 100644 --- a/Trajet.c +++ b/Trajet.c @@ -186,6 +186,15 @@ float Trajet_get_orientation_avance(){ return angle; } +void Trajet_inverse(){ + float old_abscisse = abscisse; + float old_position_mm = position_mm; + Trajectoire_inverse(&trajet_trajectoire); + Trajet_debut_trajectoire(trajet_trajectoire); + abscisse = 1 - old_abscisse; + position_mm = Trajectoire_get_longueur_mm(&trajet_trajectoire) - old_position_mm; +} + float Trajet_get_abscisse(){ return abscisse; }