From 9ffa087e2dda98cb6c01b3ad80c25d9924646c29 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 28 Apr 2024 21:58:09 +0200 Subject: [PATCH] Localisation fonctionnelle --- .vscode/settings.json | 3 +- CMakeLists.txt | 2 ++ Geometrie.c | 82 +++++++++++++++++++++++++++++++++++++++++++ Geometrie.h | 22 ++++++++++++ Geometrie_robot.h | 4 +++ Localisation.c | 54 ++++++++++++++++++++++++++++ Localisation.h | 10 ++++++ main.c | 8 ++--- 8 files changed, 180 insertions(+), 5 deletions(-) create mode 100644 Geometrie.c create mode 100644 Geometrie.h create mode 100644 Geometrie_robot.h create mode 100644 Localisation.c create mode 100644 Localisation.h diff --git a/.vscode/settings.json b/.vscode/settings.json index c988ddf..0d8ffc7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,5 +1,6 @@ { "files.associations": { - + "geometrie.h": "c", + "geometrie_robot.h": "c" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 7707fc3..889048b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,9 @@ pico_sdk_init() add_executable(Mon_Projet Asser_Moteurs.c + Geometrie.c Moteurs.c + Localisation.c main.c Temps.c QEI.c diff --git a/Geometrie.c b/Geometrie.c new file mode 100644 index 0000000..1d3bcaf --- /dev/null +++ b/Geometrie.c @@ -0,0 +1,82 @@ +#include "Geometrie.h" +#include "math.h" + + +/// @brief Retourne l'angle entre -PI et +PI +/// @param angle +/// @return +float Geometrie_get_angle_normalisee(float angle){ + while(angle > M_PI){ + angle -= 2* M_PI; + } + while(angle < -M_PI){ + angle += 2* M_PI; + } + return angle; +} + +/// @brief Indique si un angle est compris entre deux angles. Les angles doivent être entre -PI et PI. +/// @param angle : angle à comparer +/// @param angle_min : début de la fourchette +/// @param angle_max : fin de la fourchette +/// @return 1 si l'angle est compris entre min et max, 0 sinon +unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max){ + if(angle_min > angle_max){ + // cas où la fourchette comprend -PI. + if( (angle > angle_min) || (angle < angle_max)){ + return 1; + } + return 0; + }else{ + // Cas normal + if( (angle > angle_min) && (angle < angle_max)){ + return 1; + } + return 0; + } +} + +/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, +/// donne l'angle consigne pour limiter les rotations inutiles. +/// Tous les angles sont en radian +/// @param angle_depart +/// @param angle_souhaite +/// @return angle_optimal en radian +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){ + while((angle_depart - angle_souhaite) > M_PI){ + angle_souhaite += 2* M_PI; + } + while((angle_depart - angle_souhaite) < -M_PI){ + angle_souhaite -= 2* M_PI; + } + return angle_souhaite; +} + +/// @brief Indique si les deux plages d'angle se recoupent +/// @param angle1_min Début de la première plage +/// @param angle1_max Fin de la première plage +/// @param angle2_min Début de la seconde plage +/// @param angle2_max Fin de la seconde plage +/// @return 1 si les deux plages s'intersectent, 0 sinon +unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max){ + // Pour que les plages s'intersectent, soit : + // * angle1_min est compris entre angle2_min et angle2_max + // * angle1_max est compris entre angle2_min et angle2_max + // * angle2_min et angle2_max sont compris entre angle1_min et angle1_max (tester angle2_min ou angle2_max est suffisant) + if(Geometrie_compare_angle(angle1_min, angle2_min, angle2_max) || + Geometrie_compare_angle(angle1_max, angle2_min, angle2_max) || + Geometrie_compare_angle(angle2_min, angle1_min, angle1_max)){ + return 1; + } + return 0; +} + +/// @brief Déplace un point de la distance indiquée en se servant de l'angle de la position donnée. +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){ + struct position_t position_arrivée; + position_arrivée.angle_radian = position_depart.angle_radian; + position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm; + position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm; + + return position_arrivée; +} diff --git a/Geometrie.h b/Geometrie.h new file mode 100644 index 0000000..0fd96fd --- /dev/null +++ b/Geometrie.h @@ -0,0 +1,22 @@ +#ifndef GEOMETRIE_H +#define GEOMETRIE_H + +#ifndef M_PI +#define M_PI (3.14159265358979323846) +#endif + +#define DEGRE_EN_RADIAN (M_PI / 180.) +#define DISTANCE_INVALIDE (-1.) + +struct position_t{ + float x_mm, y_mm; + float angle_radian; +}; + +float Geometrie_get_angle_normalisee(float angle); +unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max); +unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max); +float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite); +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm); + +#endif diff --git a/Geometrie_robot.h b/Geometrie_robot.h new file mode 100644 index 0000000..15e2a80 --- /dev/null +++ b/Geometrie_robot.h @@ -0,0 +1,4 @@ +#include "Geometrie.h" + +#define DISTANCE_ROUES_CENTRE_MM 52. + diff --git a/Localisation.c b/Localisation.c new file mode 100644 index 0000000..f386a08 --- /dev/null +++ b/Localisation.c @@ -0,0 +1,54 @@ +#include "Localisation.h" +#include "Temps.h" +#include "QEI.h" +#include "math.h" +#include "Geometrie_robot.h" + +struct position_t position; + +void Localisation_init(){ + Temps_init(); + QEI_init(); + position.x_mm = 0; + position.y_mm = 0; + position.angle_radian = 0; +} + +void Localisation_set(float x_mm, float y_mm, float angle_radian){ + position.x_mm = x_mm; + position.y_mm = y_mm; + position.angle_radian = angle_radian; +} + +void Localisation_set_x(float x_mm){ + position.x_mm = x_mm; +} + +void Localisation_set_y(float y_mm){ + position.y_mm = y_mm; +} + +void Localisation_set_angle(float angle_radian){ + position.angle_radian = angle_radian; +} + +void Localisation_gestion(){ + float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME); + float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME); + + float delta_avance_mm, delta_orientation_rad; + + delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2; + delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2); + + position.angle_radian += delta_orientation_rad; + + // Projection dans le référentiel de la table + position.x_mm += delta_avance_mm * cosf(position.angle_radian); + position.y_mm += delta_avance_mm * sinf(position.angle_radian); + +} + +struct position_t Localisation_get(void){ + return position; +} diff --git a/Localisation.h b/Localisation.h new file mode 100644 index 0000000..12b4716 --- /dev/null +++ b/Localisation.h @@ -0,0 +1,10 @@ +#include "Geometrie.h" + +struct position_t Localisation_get(void); +void Localisation_gestion(); +void Localisation_init(); + +void Localisation_set(float x_mm, float y_mm, float angle_radian); +void Localisation_set_x(float x_mm); +void Localisation_set_y(float y_mm); +void Localisation_set_angle(float angle_radian); \ No newline at end of file diff --git a/main.c b/main.c index cd50c96..ae3447e 100644 --- a/main.c +++ b/main.c @@ -8,6 +8,7 @@ #include "hardware/adc.h" #include "hardware/pwm.h" #include "Asser_Moteurs.h" +#include "Localisation.h" #include "Moteurs.h" #include "Temps.h" #include @@ -36,19 +37,16 @@ void main(void) Temps_init(); tension_batterie_init(); identifiant_init(); + Localisation_init(); uint32_t temps_ms = Temps_get_temps_ms(); gpio_init(LED1PIN); gpio_set_dir(LED1PIN, GPIO_OUT ); gpio_put(LED1PIN, 1); - AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100.); - AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100.); multicore_launch_core1(affichage); - Moteur_SetVitesse(MOTEUR_A, 000); - Moteur_SetVitesse(MOTEUR_B, 000); while(1){ @@ -58,6 +56,7 @@ void main(void) if(temps_ms % step_ms == 0){ QEI_update(); AsserMoteur_Gestion(step_ms); + Localisation_gestion(); } if(temps_ms % 100 == 0){ identifiant_lire(); @@ -76,6 +75,7 @@ void affichage(void){ /*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) ); printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ + printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); sleep_ms(100); } }