diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 05ca5f8..2996c7e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -2,6 +2,7 @@ "env": { "myDefaultIncludePath": [ "${workspaceFolder}", + "${workspaceFolder}/VL53L8CX_ULD_API/inc", "${workspaceFolder}/build", "${env:PICO_SDK_PATH}/src/**/include", "${env:PICO_SDK_PATH}/src/common/pico_base/include", diff --git a/CMakeLists.txt b/CMakeLists.txt index 74ff8cf..ab5a3a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,6 +26,7 @@ add_executable(Mon_Projet Temps.c Trajectoire_bezier.c Trajectoire_circulaire.c + Trajectoire_composees.c Trajectoire_droite.c Trajectoire.c Trajet.c diff --git a/Strategie.c b/Strategie.c index 94437c8..6c777d6 100644 --- a/Strategie.c +++ b/Strategie.c @@ -5,46 +5,31 @@ enum etat_action_t Strategie_super_star(uint32_t step_ms){ static enum{ - SSS_TEST_TOURNE, SSS_INIT, - SSS_AVANCE_1, - SSS_TOURNE, - SSS_AVANCE_2, + SSS_AVANCE, SSS_DANCE } etat_sss = SSS_INIT; - struct trajectoire_t trajectoire; + static struct trajectoire_t trajectoire_composee; + static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3; + + switch(etat_sss){ - case SSS_TEST_TOURNE: - Localisation_set(1135, 1895, 0); - etat_sss = SSS_TOURNE; - break; case SSS_INIT: Localisation_set(45, 1895, 0); - etat_sss = SSS_AVANCE_1; + Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0); + Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); + Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0); + Trajectoire_composee_init(&trajectoire_composee); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2); + Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3); + etat_sss = SSS_AVANCE; break; - case SSS_AVANCE_1: - Trajet_config(TRAJECT_CONFIG_STD); - Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0); - if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ - etat_sss = SSS_TOURNE; - } - break; - - case SSS_TOURNE: - //Trajet_config(100, 100); - Trajet_config(TRAJECT_CONFIG_STD); - Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2); - if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ - etat_sss = SSS_AVANCE_2; - } - break; - - case SSS_AVANCE_2: - Trajet_config(TRAJECT_CONFIG_STD); - Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1580, 0, 0); - if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ + case SSS_AVANCE: + Trajet_config(TRAJECT_CONFIG_RAPIDE); + if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_sss = SSS_DANCE; } break; diff --git a/Trajectoire.c b/Trajectoire.c index 547fb00..063ec36 100644 --- a/Trajectoire.c +++ b/Trajectoire.c @@ -2,15 +2,15 @@ #include "Trajectoire_bezier.h" #include "Trajectoire_circulaire.h" #include "Trajectoire_droite.h" +#include "Trajectoire_composees.h" #include "math.h" -#define NB_MAX_TRAJECTOIRES 5 #define PRECISION_ABSCISSE 0.001f -void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, float angle_fin_rad, float rayon, - float orientation_debut_rad, float orientation_fin_rad){ +void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad, + float angle_fin_rad, float rayon, float orientation_debut_rad, float orientation_fin_rad){ trajectoire->type = TRAJECTOIRE_CIRCULAIRE; trajectoire->p1.x = centre_x; trajectoire->p1.y = centre_y; @@ -34,8 +34,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1 trajectoire->orientation_fin_rad = orientation_fin_rad; } -void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, - float orientation_debut_rad, float orientation_fin_rad){ +void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, + float p4_x, float p4_y, float orientation_debut_rad, float orientation_fin_rad){ trajectoire->type = TRAJECTOIRE_BEZIER; trajectoire->p1.x = p1_x; trajectoire->p1.y = p1_y; @@ -50,6 +50,27 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1 trajectoire->orientation_fin_rad = orientation_fin_rad; } +/// @brief Initialise une trajectoire composée +/// @param trajectoire : trajectoire à initialiser +void Trajectoire_composee_init(struct trajectoire_t * trajectoire){ + trajectoire->type = TRAJECTOIRE_COMPOSEE; + trajectoire->longueur = -1; + trajectoire->nb_trajectoire = 0; + for (int index =0; index trajectoires[index] = NULL; + } +} + +/// @brief Ajoute une trajectoire à une trajectoire composée déjà initialisée +/// @param trajectoire_composee +/// @param trajectoire +void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire){ + if(trajectoire_composee->nb_trajectoire < NB_MAX_TRAJECTOIRE){ + trajectoire_composee->trajectoires[trajectoire_composee->nb_trajectoire] = trajectoire; + trajectoire_composee->nb_trajectoire++; + } +} + void Trajectoire_inverse(struct trajectoire_t * trajectoire){ struct trajectoire_t old_trajectoire; old_trajectoire = *trajectoire; @@ -97,6 +118,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ case TRAJECTOIRE_BEZIER: Trajectoire_bezier_get_longueur(trajectoire); break; + + case TRAJECTOIRE_COMPOSEE: + Trajectoire_composee_get_longueur(trajectoire); + break; } } return trajectoire->longueur; @@ -123,6 +148,10 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse); break; + case TRAJECTOIRE_COMPOSEE: + point_xyo = Trajectoire_composee_get_point(trajectoire, abscisse); + break; + } return point_xyo; } diff --git a/Trajectoire.h b/Trajectoire.h index 5cf1bb3..a072781 100644 --- a/Trajectoire.h +++ b/Trajectoire.h @@ -1,10 +1,13 @@ #ifndef TRAJECTOIRE_H #define TRAJECTOIRE_H +#define NB_MAX_TRAJECTOIRE 10 + enum trajectoire_type_t{ TRAJECTOIRE_DROITE, TRAJECTOIRE_CIRCULAIRE, TRAJECTOIRE_BEZIER, + TRAJECTOIRE_COMPOSEE }; struct point_xy_t{ @@ -22,6 +25,9 @@ struct trajectoire_t { float orientation_debut_rad, orientation_fin_rad; float rayon, angle_debut_rad, angle_fin_rad; float longueur; + // Pour les trajectoires composées + struct trajectoire_t * trajectoires[NB_MAX_TRAJECTOIRE]; + int nb_trajectoire; }; float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire); @@ -35,6 +41,9 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1 void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y, float orientation_debut_rad, float orientation_fin_rad); void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad); +void Trajectoire_composee_init(struct trajectoire_t * trajectoire); +void Trajectoire_composee_ajout(struct trajectoire_t * trajectoire_composee, struct trajectoire_t * trajectoire); + void Trajectoire_inverse(struct trajectoire_t * trajectoire); #endif diff --git a/Trajectoire_composees.c b/Trajectoire_composees.c new file mode 100644 index 0000000..38428c2 --- /dev/null +++ b/Trajectoire_composees.c @@ -0,0 +1,24 @@ +#include +#include "Trajectoire.h" +#include "Trajectoire_composees.h" + +void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee){ + trajectoire_composee->longueur = 0; + for(unsigned int i = 0 ; inb_trajectoire ; i++) { + trajectoire_composee->longueur += Trajectoire_get_longueur_mm(trajectoire_composee->trajectoires[i]); + } +} + + +struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse){ + // Obtenir la trajectoire sur laquelle nous sommes... + int index_trajectoire; + double abscisse_etendue, abscisse_locale; + + abscisse_etendue = abscisse * trajectoire_composee->nb_trajectoire; + index_trajectoire = (int) floor(abscisse_etendue); + abscisse_locale = abscisse_etendue - floor(abscisse_etendue); + + return Trajectoire_get_point(trajectoire_composee->trajectoires[index_trajectoire], abscisse_locale); +} + diff --git a/Trajectoire_composees.h b/Trajectoire_composees.h new file mode 100644 index 0000000..ef7d441 --- /dev/null +++ b/Trajectoire_composees.h @@ -0,0 +1,3 @@ + +struct point_xyo_t Trajectoire_composee_get_point(struct trajectoire_t * trajectoire_composee, double abscisse); +void Trajectoire_composee_get_longueur(struct trajectoire_t * trajectoire_composee);