From e535b32caf5ac402b36917878efd985c1d08fe28 Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 6 Jul 2026 22:57:15 +0200 Subject: [PATCH] =?UTF-8?q?Initialisation=20du=20d=C3=A9p=C3=B4t?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Asser_Moteurs.c | 106 +++++++++++++++++++ Asser_Moteurs.h | 9 ++ Asser_Position.c | 78 ++++++++++++++ Asser_Position.h | 6 ++ CMakeLists.txt | 29 ++++++ Commande_vitesse.c | 36 +++++++ Commande_vitesse.h | 4 + Localisation.c | 54 ++++++++++ Localisation.h | 10 ++ Moteurs.c | 141 +++++++++++++++++++++++++ Moteurs.h | 15 +++ QEI.c | 114 ++++++++++++++++++++ QEI.h | 16 +++ Readme.md | 65 ++++++++++++ Rotation.c | 88 ++++++++++++++++ Rotation.h | 4 + Trajectoire.c | 196 +++++++++++++++++++++++++++++++++++ Trajectoire.h | 48 +++++++++ Trajectoire_bezier.c | 35 +++++++ Trajectoire_bezier.h | 5 + Trajectoire_circulaire.c | 26 +++++ Trajectoire_circulaire.h | 4 + Trajectoire_composees.c | 24 +++++ Trajectoire_composees.h | 3 + Trajectoire_droite.c | 17 +++ Trajectoire_droite.h | 4 + Trajet.c | 219 +++++++++++++++++++++++++++++++++++++++ Trajet.h | 37 +++++++ 28 files changed, 1393 insertions(+) create mode 100644 Asser_Moteurs.c create mode 100644 Asser_Moteurs.h create mode 100644 Asser_Position.c create mode 100644 Asser_Position.h create mode 100644 CMakeLists.txt create mode 100644 Commande_vitesse.c create mode 100644 Commande_vitesse.h create mode 100644 Localisation.c create mode 100644 Localisation.h create mode 100644 Moteurs.c create mode 100644 Moteurs.h create mode 100644 QEI.c create mode 100644 QEI.h create mode 100644 Readme.md create mode 100644 Rotation.c create mode 100644 Rotation.h create mode 100644 Trajectoire.c create mode 100644 Trajectoire.h create mode 100644 Trajectoire_bezier.c create mode 100644 Trajectoire_bezier.h create mode 100644 Trajectoire_circulaire.c create mode 100644 Trajectoire_circulaire.h create mode 100644 Trajectoire_composees.c create mode 100644 Trajectoire_composees.h create mode 100644 Trajectoire_droite.c create mode 100644 Trajectoire_droite.h create mode 100644 Trajet.c create mode 100644 Trajet.h diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c new file mode 100644 index 0000000..749f44a --- /dev/null +++ b/Asser_Moteurs.c @@ -0,0 +1,106 @@ +#include "config_robot.h" +#include "QEI.h" +#include "Moteurs.h" +#include "Asser_Moteurs.h" + +// Paramètres pour PAMI +#ifdef ROBOT_TYPE_PAMI +#define ASSERMOTEUR_GAIN_P 30000.f +#define ASSERMOTEUR_GAIN_I 3000.f +#endif + +// Paramètre Robot 2026 +#ifdef ROBOT_PROPULSION_2026 +#define ASSERMOTEUR_GAIN_P 150.f +#define ASSERMOTEUR_GAIN_I 1.f +#endif + + +float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) +float commande_I[3]; // Terme integral + +void AsserMoteur_Init(int id){ + QEI_init(id); + Moteur_Init(); + for(unsigned int i =0; i< 2; i ++){ + commande_I[i]=0; + consigne_mm_s[i]=0; + } +} + +/// @brief Défini une consigne de vitesse pour le moteur indiqué. +/// @param moteur : Moteur à asservir +/// @param _consigne_mm_s : consigne de vitesse en mm/s +void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){ + consigne_mm_s[moteur] = _consigne_mm_s; + +} + +/// @brief Envoie la consigne du moteur +/// @param moteur : Moteur à asservir +float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ + return consigne_mm_s[moteur]; +} + + + +float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ + enum QEI_name_t qei; + float distance, temps; + switch (moteur) + { + case MOTEUR_A: qei = QEI_A_NAME; break; + case MOTEUR_B: qei = QEI_B_NAME; break; + default: break; + } + distance = QEI_get_mm(qei); + temps = step_ms / 1000.0f; + + 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 ){ + return 1; + } + return 0; +} + +void AsserMoteurs_stop(void){ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); +} + +/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement +/// @param step_ms +void AsserMoteur_Gestion(int step_ms){ + // Pour chaque moteur + for(uint moteur=MOTEUR_A; moteur 32760) {commande = 32760;} + if(commande < -32760) {commande = -32760;} + + Moteur_SetVitesse(moteur, commande); + + } +} \ No newline at end of file diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h new file mode 100644 index 0000000..dcdfe8f --- /dev/null +++ b/Asser_Moteurs.h @@ -0,0 +1,9 @@ +#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); +void AsserMoteur_Gestion(int step_ms); +void AsserMoteur_Init(int); +void AsserMoteurs_stop(void); \ No newline at end of file diff --git a/Asser_Position.c b/Asser_Position.c new file mode 100644 index 0000000..049a646 --- /dev/null +++ b/Asser_Position.c @@ -0,0 +1,78 @@ +#include "Localisation.h" +#include "Commande_vitesse.h" +#include "math.h" + +#define GAIN_P_POSITION 5 +#define GAIN_P_ORIENTATION 5 + +#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN) + +struct position_t position_maintien; + +/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot +/// C'est à la consigne d'être défini avant pour être atteignable. +/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms); +/// @param position_consigne : position à atteindre dans le référentiel de la table. +float delta_x_mm, delta_y_mm, delta_orientation_radian; +float delta_orientation_radian_tmp; +void Asser_Position(struct position_t position_consigne){ + float delta_avance_mm; + float avance_mm_s, rotation_radian_s; + //float delta_x_mm, delta_y_mm, delta_orientation_radian; + struct position_t position_actuelle; + float delta_orientation_radian_tmp; + + position_actuelle = Localisation_get(); + + // Calcul de l'erreur + delta_x_mm = position_consigne.x_mm - position_actuelle.x_mm; + delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm; + delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm); + delta_orientation_radian_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian; + delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp); + + // On asservi sur +PI/2 / -PI/2 + if(delta_orientation_radian > (M_PI/2)){ + delta_orientation_radian -= M_PI; + delta_avance_mm = -delta_avance_mm; + } + if(delta_orientation_radian < -(M_PI/2)){ + delta_orientation_radian += M_PI; + delta_avance_mm = -delta_avance_mm; + } + + + + // Asservissement + avance_mm_s = delta_avance_mm * GAIN_P_POSITION; + rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION; + + /*if(delta_avance_mm < 10){ + rotation_radian_s=delta_avance_mm/10 * rotation_radian_s; + }*/ + + // Commande en vitesse + commande_vitesse(avance_mm_s, rotation_radian_s); + +} + +void Asser_Position_set_Pos_Maintien(struct position_t position){ + position_maintien=position; +} + +void Asser_Position_maintien(){ + Asser_Position(position_maintien); +} + +float Asser_Position_get_erreur_angle(){ + return delta_orientation_radian; +} + +/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil +/// @return 1 si panic, 0 si nominal +int Asser_Position_panic_angle(){ + if(delta_orientation_radian > MAX_ERREUR_ANGLE){ + return 1; + } + return 0; +} diff --git a/Asser_Position.h b/Asser_Position.h new file mode 100644 index 0000000..aa2e802 --- /dev/null +++ b/Asser_Position.h @@ -0,0 +1,6 @@ +#include "Geometrie.h" +void Asser_Position(struct position_t position_consigne); +void Asser_Position_set_Pos_Maintien(struct position_t position); +void Asser_Position_maintien(); + +int Asser_Position_panic_angle(); diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..74b0113 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,29 @@ +project(Deplacement_Robot_differentiel C) + +include(../pico_sdk_import.cmake) + +add_library(Deplacement_Robot_differentiel + Asser_Position.c + Asser_Moteurs.c + Commande_vitesse.c + Evitement.c + Moteurs.c + Localisation.c + QEI.c + Rotation.c + Trajectoire_bezier.c + Trajectoire_circulaire.c + Trajectoire_composees.c + Trajectoire_droite.c + Trajectoire.c + Trajet.c +) + +pico_generate_pio_header(Deplacement_Robot_differentiel ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) + +target_link_libraries(Deplacement_Robot_differentiel + hardware_pio + hardware_pwm + pico_stdlib + +) diff --git a/Commande_vitesse.c b/Commande_vitesse.c new file mode 100644 index 0000000..2050702 --- /dev/null +++ b/Commande_vitesse.c @@ -0,0 +1,36 @@ +#include "Asser_Moteurs.h" +#include "Geometrie_robot.h" +#include "Commande_vitesse.h" + + +float avance_mm_s, orientation_radian_s; + +float get_avance_mm_s(){ + return avance_mm_s; +} + +float get_orientation_radian_s(){ + return orientation_radian_s; +} + +/// @brief Commande de la vitesse dans le référentiel du robot +/// @param avance_mm_s : Vitesse d'avance +/// @param orientation_radian_s : Rotation en radian/s +void commande_vitesse(float _avance_mm_s, float _orientation_radian_s){ + float vitesse_roue_gauche, vitesse_roue_droite; + avance_mm_s = _avance_mm_s; + orientation_radian_s = _orientation_radian_s; + + vitesse_roue_gauche = avance_mm_s - (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM); + vitesse_roue_droite = avance_mm_s + (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM); + + AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_droite); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_gauche); + +} + +/// @brief Arrête le robot. +void commande_vitesse_stop(){ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0); +} \ No newline at end of file diff --git a/Commande_vitesse.h b/Commande_vitesse.h new file mode 100644 index 0000000..617e75b --- /dev/null +++ b/Commande_vitesse.h @@ -0,0 +1,4 @@ +void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s); +void commande_vitesse_stop(void); +float get_avance_mm_s(); +float get_orientation_radian_s(); \ No newline at end of file diff --git a/Localisation.c b/Localisation.c new file mode 100644 index 0000000..e2e735e --- /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(int id){ + Temps_init(); + QEI_init(id); + 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..de101f6 --- /dev/null +++ b/Localisation.h @@ -0,0 +1,10 @@ +#include "Geometrie.h" + +struct position_t Localisation_get(void); +void Localisation_gestion(); +void Localisation_init(int); + +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/Moteurs.c b/Moteurs.c new file mode 100644 index 0000000..9affc47 --- /dev/null +++ b/Moteurs.c @@ -0,0 +1,141 @@ +#include "config_robot.h" +#include "hardware/pwm.h" +#include "Moteurs.h" + +#define MOTEUR_A 0 +#define MOTEUR_B 1 +#define MOTEUR_C 2 + +#ifdef ROBOT_PROPULSION_2026 + #define M1_VITESSE 2 //1A + #define M1_SENS1 3 + #define M1_SENS2 4 + #define M2_VITESSE 6 //3A + #define M2_SENS1 7 + #define M2_SENS2 8 +#else + #define M1_SENS1 7 + #define M1_SENS2 13 + #define M1_VITESSE 27 //5B + #define M2_SENS1 10 + #define M2_SENS2 5 + #define M2_VITESSE 9 //4B +#endif + +uint slice_moteur_A, slice_moteur_B, slice_moteur_C; +int moteur_a_pwm, moteur_b_pwm; + +/// @brief Initialisation les entrées / sorties requises pour les moteurs +void Moteur_Init(){ + gpio_init(M1_SENS1); + gpio_init(M1_SENS2); + gpio_init(M2_SENS1); + gpio_init(M2_SENS2); + gpio_set_dir(M1_SENS1, GPIO_OUT); + gpio_set_dir(M1_SENS2, GPIO_OUT); + gpio_set_dir(M2_SENS1, GPIO_OUT); + gpio_set_dir(M2_SENS2, GPIO_OUT); + gpio_put(M1_SENS1, 0); + gpio_put(M1_SENS2, 0); + gpio_put(M2_SENS1, 0); + gpio_put(M2_SENS2, 0); + + gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM); + gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM); +#ifdef ROBOT_PROPULSION_2026 + pwm_set_wrap(1, (uint16_t)65535); + pwm_set_wrap(3, (uint16_t)65535); + pwm_set_chan_level(1, PWM_CHAN_A, 0); + pwm_set_chan_level(3, PWM_CHAN_A, 0); + pwm_set_enabled(1, true); + pwm_set_enabled(3, true); +#else + pwm_set_wrap(4, (uint16_t)65535); + pwm_set_wrap(5, (uint16_t)65535); + pwm_set_chan_level(4, PWM_CHAN_B, 0); + pwm_set_chan_level(5, PWM_CHAN_B, 0); + pwm_set_enabled(4, true); + pwm_set_enabled(5, true); +#endif + Moteur_SetVitesse(MOTEUR_A, 0); + Moteur_SetVitesse(MOTEUR_B, 0); +} + +/// @brief Renvoie la commande signée sur 16 bits du PWM +/// @param moteur MOTEUR_A ou MOTEUR_B +/// @return +int16_t Moteur_GetVitesse(enum t_moteur moteur){ + uint16_t u_vitesse; + switch (moteur) + { + case MOTEUR_A: + return moteur_a_pwm; + break; + + case MOTEUR_B: + return moteur_b_pwm; + break; + + default: + return 0; + break; + } +} + + +/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur +/// @param moteur : Moteur (voir enum t_moteur) +/// @param vitesse : Vitesse entre -32767 et 32767 +void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ + uint16_t u_vitesse; + + // Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe) + if (vitesse < 0){ + u_vitesse = -vitesse; + }else{ + u_vitesse = vitesse; + } + u_vitesse = u_vitesse * 2; + + switch(moteur){ + case MOTEUR_A: + +#ifdef ROBOT_PROPULSION_2026 + pwm_set_chan_level(1, PWM_CHAN_A, u_vitesse); + if(vitesse > 0){ +#else + pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse); + if(vitesse < 0){ +#endif + gpio_put(M1_SENS1, 1); + gpio_put(M1_SENS2, 0); + }else{ + gpio_put(M1_SENS1, 0); + gpio_put(M1_SENS2, 1); + } + break; + + case MOTEUR_B: +#ifdef ROBOT_PROPULSION_2026 + pwm_set_chan_level(3, PWM_CHAN_A, u_vitesse); + if(vitesse < 0){ +#else + pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse); + if(vitesse > 0){ +#endif + gpio_put(M2_SENS1, 1); + gpio_put(M2_SENS2, 0); + }else{ + gpio_put(M2_SENS1, 0); + gpio_put(M2_SENS2, 1); + } + break; + + } + +} + +void Moteur_Stop(void){ + Moteur_SetVitesse(MOTEUR_A, 0); + Moteur_SetVitesse(MOTEUR_B, 0); +} \ No newline at end of file diff --git a/Moteurs.h b/Moteurs.h new file mode 100644 index 0000000..a072484 --- /dev/null +++ b/Moteurs.h @@ -0,0 +1,15 @@ +#include "pico/stdlib.h" + +#ifndef MOTEURS_H +#define MOTEURS_H +enum t_moteur { + MOTEUR_A=0, + MOTEUR_B=1, + MOTEUR_C=2 +}; +#endif + +void Moteur_Init(void); +void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ); +int16_t Moteur_GetVitesse(enum t_moteur moteur); +void Moteur_Stop(void); diff --git a/QEI.c b/QEI.c new file mode 100644 index 0000000..70e9f2b --- /dev/null +++ b/QEI.c @@ -0,0 +1,114 @@ +#include "config_robot.h" +#include +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/timer.h" +#include "QEI.h" +#include "quadrature_encoder.pio.h" + + +/*** C'est ici que se fait la conversion en mm + * ***/ + +// Roues 60 mm de diamètre, 188,5 mm de circonférence +// Réduction Moteur 30:1 +// Réduction poulie 16:12 +// Nombre d'impulsions par tour moteur : 200 +// Nombre d'impulsions par tour réducteur : 6000 +// Nombre d'impulsions par tour de roue : 8000 +// Impulsion / mm : 42,44 + +#define IMPULSION_PAR_MM_50_1 (12.45f) +#define IMPULSION_PAR_MM_30_1 (7.47f) +#define IMPULSION_PAR_MM_robot_2026 (7.57f) + +float impulsion_par_mm; + + +struct QEI_t QEI_A, QEI_B; + +bool QEI_est_init = false; + +PIO pio_QEI = pio0; + + +void QEI_init(int identifiant){ + // Initialisation des 3 modules QEI + // Chaque module QEI sera dans une machine à état du PIO 0 + if(!QEI_est_init){ + // Offset le début du programme + // Si ce n'est pas 0, le programme ne marchera pas + uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); + if(offset != 0){ + printf("PIO init error: offset != 0"); + } + // Initialisation des "machines à états" : +#ifdef ROBOT_PROPULSION_2026 + // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 10 et 11, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 1, offset, 10, 0); + // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 14 et 15, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 0, offset, 14, 0); +#else + // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0); + // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer + quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0); +#endif + + QEI_A.value=0; + QEI_B.value=0; + QEI_est_init=true; + } +#ifdef ROBOT_PROPULSION_2026 + impulsion_par_mm = IMPULSION_PAR_MM_robot_2026; +#else + impulsion_par_mm = IMPULSION_PAR_MM_50_1; + if(identifiant == 0 || identifiant >= 4){ + impulsion_par_mm = IMPULSION_PAR_MM_30_1; + } +#endif + +} + +/// @brief Lit les modules QEI et stock l'écart en cette lecture et la lecture précédente. +void QEI_update(void){ + + int old_value; + + old_value = QEI_A.value; + QEI_A.value = quadrature_encoder_get_count(pio_QEI, 0); + QEI_A.delta = QEI_A.value - old_value; + + old_value = QEI_B.value; + QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1); + QEI_B.delta = QEI_B.value - old_value; + +} + +/// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente +/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation. +/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique +/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) +/// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update() +int QEI_get(enum QEI_name_t qei){ + switch (qei) + { + case QEI_A_NAME: + return QEI_A.delta; + break; + + case QEI_B_NAME: + return -QEI_B.delta; + break; + + default: + break; + } +} + +/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente +/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) +/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update() +float QEI_get_mm(enum QEI_name_t qei){ + return ((float) QEI_get(qei)) / impulsion_par_mm; +} \ No newline at end of file diff --git a/QEI.h b/QEI.h new file mode 100644 index 0000000..e031360 --- /dev/null +++ b/QEI.h @@ -0,0 +1,16 @@ +struct QEI_t{ + int value; + int delta; +}; + +enum QEI_name_t{ + QEI_A_NAME=0, + QEI_B_NAME=1, +}; + +extern struct QEI_t QEI_A, QEI_B, QEI_C; + +void QEI_update(void); +void QEI_init(int); +int QEI_get(enum QEI_name_t qei); +float QEI_get_mm(enum QEI_name_t qei); \ No newline at end of file diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..812f36e --- /dev/null +++ b/Readme.md @@ -0,0 +1,65 @@ +Submodule Servomoteurs pour le RP2040 +===================================== + +Ceci est un submodule git pour intégrer facilement le support des servomoteurs à un projet. +On crée ce Submodule en se servant de la documentation disponible ici : https://git-scm.com/book/en/v2/Git-Tools-Submodules + +Utilisation +=========== + +Ajout du sub-module +------------------- + +Ajouter ce submodule à votre projet. Dans un terminal, dans le répertoire de votre projet, entrez : + + git submodule add https://git.poivron-robotique.fr/Keuronde/Module_deplacement_robot_differentiel.git + +Ajouter le fichier nouvellement créé .gitmodules à votre projet. + + git add .gitmodules + +Configuration de la compilation +------------------------------- + +Dans le fichier CMakeLists.txt, ajouter le dossier du sub module: + + add_subdirectory(RP2040_Servomoteurs) + +Dans le fichier CMakeLists.txt, ajouter la bibliothèque RP2040_Servomoteur : + + target_link_libraries(Modele_RPiPico + ... + RP2040_Servomoteur + ) + +Intégration au code source +-------------------------- + +Ajout du fichier d'include : + + #include "RP2040_Servomoteurs/Servomoteur.h" + +Initilisation du module : + + Servomoteur_Init(); + +Envoie d'une consigne au servomoteur : + + Servomoteur_set(num_gpio, position); + +Avec : +- num_gpio : le numéro de la GPIO du RP2040 +- position : la position du servomoteur, qui correspond à un temps haut du signal compris généralement en 0,5 et 2,5 ms (en fonction des servomoteurs). le fichier _Servomoteur.h_ contient des valeurs d'exemple. + +Cas où vous clonez un projet contenant des submodules +----------------------------------------------------- + +Après avoir cloné le projet, initilisez les submodules: + + git submodule init + + git submodule update + +Pour récupérer les dernières mise à jour des sub-modules : + + git submodule update --remote diff --git a/Rotation.c b/Rotation.c new file mode 100644 index 0000000..e2641e4 --- /dev/null +++ b/Rotation.c @@ -0,0 +1,88 @@ +#include "Strategie.h" +#include "Rotation.h" +#include "Localisation.h" +#include "Asser_Moteurs.h" +#include "Geometrie_robot.h" +#include "math.h" + +float rotation_angle_cible; +float rotation_angle_sens; +float rotation_vitesse_rad_s; +float rotation_vitesse_max_rad_s; +float rotation_acceleration_rad_ss; + +float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s); + +void rotation_init(float angle_cible){ + struct position_t position = Localisation_get(); + rotation_angle_cible = Geometrie_get_angle_optimal(position.angle_radian, angle_cible); + rotation_vitesse_rad_s = 0; + rotation_vitesse_max_rad_s = 5; + rotation_acceleration_rad_ss = 25; + + if(rotation_angle_cible - position.angle_radian > 0){ + rotation_angle_sens = 1; + }else{ + rotation_angle_sens = -1; + } + +} + +void rotation_set_vitesse(float vitesse_rad_s){ + float vitesse_mm_s = vitesse_rad_s * DISTANCE_ROUES_CENTRE_MM; + if(rotation_angle_sens == 1){ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_mm_s); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, -vitesse_mm_s); + }else{ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, -vitesse_mm_s); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_mm_s); + } +} +enum etat_action_t rotation_gestion(float pas_de_temps_s){ + struct position_t position = Localisation_get(); + rotation_vitesse_rad_s = Rotation_calcul_vitesse(position.angle_radian, pas_de_temps_s); + + if (rotation_angle_sens == 1 ){ + rotation_set_vitesse(rotation_vitesse_rad_s); + if (position.angle_radian > rotation_angle_cible){ + return ACTION_TERMINEE; + } + + }else{ + if (position.angle_radian < rotation_angle_cible){ + return ACTION_TERMINEE; + } + } + return ACTION_EN_COURS; + +} + +float Rotation_calcul_vitesse(float angle_actuel, float pas_de_temps_s){ + // Phase d'accélération + rotation_vitesse_rad_s = rotation_vitesse_rad_s + pas_de_temps_s * rotation_acceleration_rad_ss; + + // Saturation par vitesse max + if(rotation_vitesse_rad_s > rotation_vitesse_max_rad_s){ + rotation_vitesse_rad_s = rotation_vitesse_max_rad_s; + } + + // Saturation par la courbe de décélération + float vitesse_décélération_rad_s; + // distance restante + float angle_restant; + if (rotation_angle_sens == 1){ + angle_restant = rotation_angle_cible - angle_actuel; + }else{ + angle_restant = angle_actuel - rotation_angle_cible; + } + + if(angle_restant < 0){ + vitesse_décélération_rad_s = 0; + }else{ + vitesse_décélération_rad_s = sqrtf(2 * rotation_acceleration_rad_ss * angle_restant); + } + if(rotation_vitesse_rad_s > vitesse_décélération_rad_s){ + rotation_vitesse_rad_s = vitesse_décélération_rad_s; + } + return rotation_vitesse_rad_s; +} diff --git a/Rotation.h b/Rotation.h new file mode 100644 index 0000000..a7efa6a --- /dev/null +++ b/Rotation.h @@ -0,0 +1,4 @@ + +extern float rotation_angle_cible, rotation_vitesse_rad_s; +void rotation_init(float angle_cible); +enum etat_action_t rotation_gestion(float pas_de_temps_s); \ No newline at end of file diff --git a/Trajectoire.c b/Trajectoire.c new file mode 100644 index 0000000..5d56985 --- /dev/null +++ b/Trajectoire.c @@ -0,0 +1,196 @@ +#include "Trajectoire.h" +#include "Trajectoire_bezier.h" +#include "Trajectoire_circulaire.h" +#include "Trajectoire_droite.h" +#include "Trajectoire_composees.h" + +#include "math.h" + +#define PRECISION_ABSCISSE 0.001f +#define NB_MAX_ITERATIONS 3 + + +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; + trajectoire->angle_debut_rad = angle_debut_rad; + trajectoire->angle_fin_rad = angle_fin_rad; + trajectoire->rayon = rayon; + trajectoire->longueur = -1; + trajectoire->orientation_debut_rad = orientation_debut_rad; + trajectoire->orientation_fin_rad = orientation_fin_rad; +} + +void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, + float orientation_debut_rad, float orientation_fin_rad){ + trajectoire->type = TRAJECTOIRE_DROITE; + trajectoire->p1.x = p1_x; + trajectoire->p1.y = p1_y; + trajectoire->p2.x = p2_x; + trajectoire->p2.y = p2_y; + trajectoire->longueur = -1; + trajectoire->orientation_debut_rad = orientation_debut_rad; + 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){ + trajectoire->type = TRAJECTOIRE_BEZIER; + trajectoire->p1.x = p1_x; + trajectoire->p1.y = p1_y; + trajectoire->p2.x = p2_x; + trajectoire->p2.y = p2_y; + trajectoire->p3.x = p3_x; + trajectoire->p3.y = p3_y; + trajectoire->p4.x = p4_x; + trajectoire->p4.y = p4_y; + trajectoire->longueur = -1; + trajectoire->orientation_debut_rad = orientation_debut_rad; + 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; + + trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad; + trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad; + + if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){ + trajectoire->angle_debut_rad = old_trajectoire.angle_fin_rad; + trajectoire->angle_fin_rad = old_trajectoire.angle_debut_rad; + return; + } + if(trajectoire->type == TRAJECTOIRE_DROITE){ + trajectoire->p1 = old_trajectoire.p2; + trajectoire->p2 = old_trajectoire.p1; + return; + } + if(trajectoire->type == TRAJECTOIRE_BEZIER){ + trajectoire->p1 = old_trajectoire.p4; + trajectoire->p2 = old_trajectoire.p3; + trajectoire->p3 = old_trajectoire.p2; + trajectoire->p4 = old_trajectoire.p1; + } + + + +} + +/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin +/// @param trajectoire +/// @return Longueur de la trajectoire +float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){ + if(trajectoire->longueur > 0){ + // La longueur est déjà calculée + }else{ + // Calculons la longueur de la trajectoire + switch(trajectoire->type){ + case TRAJECTOIRE_DROITE: + Trajectoire_droite_get_longueur(trajectoire); + break; + case TRAJECTOIRE_CIRCULAIRE: + Trajectoire_circulaire_get_longueur(trajectoire); + break; + + case TRAJECTOIRE_BEZIER: + Trajectoire_bezier_get_longueur(trajectoire); + break; + + case TRAJECTOIRE_COMPOSEE: + Trajectoire_composee_get_longueur(trajectoire); + break; + } + } + return trajectoire->longueur; +} + +/// @brief Renvoie le point d'une trajectoire à partir de son abscisse +/// @param abscisse : abscisse sur la trajectoire +/// @return point en coordonnées X/Y +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){ + struct point_xyo_t point_xyo; + switch(trajectoire->type){ + case TRAJECTOIRE_DROITE: + point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse); + break; + + case TRAJECTOIRE_CIRCULAIRE: + point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse); + break; + + case TRAJECTOIRE_BEZIER: + point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse); + break; + + case TRAJECTOIRE_COMPOSEE: + point_xyo = Trajectoire_composee_get_point(trajectoire, abscisse); + break; + + } + return point_xyo; +} + +float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){ + return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse; +} + +/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée +/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire +/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire +/// @return nouvelle abscisse +float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){ + double delta_abscisse, delta_mm, erreur_relative; + + if(distance_mm == 0){ + return abscisse; + } + // Ceci permet d'avoir une abscisse exact sur les trajectoires droites, les trajectoires circulaires et les rotations + delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire); + if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE || trajectoire->type == TRAJECTOIRE_DROITE){ + return abscisse + delta_abscisse; + } + + delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy ); + + // Sur les trajectoires de bézier, il peut être nécessaire d'affiner + // Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire. + erreur_relative = 1 - delta_mm / distance_mm; + int nb_iteration=0; + while(fabs(erreur_relative) > PRECISION_ABSCISSE && nb_iteration < NB_MAX_ITERATIONS){ + delta_abscisse = delta_abscisse * distance_mm / delta_mm; + delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy ); + erreur_relative = 1 - delta_mm / distance_mm; + nb_iteration++; + } + + return abscisse + delta_abscisse; +} + +double distance_points(struct point_xy_t point, struct point_xy_t point_old){ + return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2)); + +} diff --git a/Trajectoire.h b/Trajectoire.h new file mode 100644 index 0000000..fc8579e --- /dev/null +++ b/Trajectoire.h @@ -0,0 +1,48 @@ +#ifndef TRAJECTOIRE_H +#define TRAJECTOIRE_H + +#define NB_MAX_TRAJECTOIRE 10 + +enum trajectoire_type_t{ + TRAJECTOIRE_DROITE=0, + TRAJECTOIRE_CIRCULAIRE=1, + TRAJECTOIRE_BEZIER=2, + TRAJECTOIRE_COMPOSEE=3 +}; + +struct point_xy_t{ + float x, y; +}; + +struct point_xyo_t{ + struct point_xy_t point_xy; +}; + +struct trajectoire_t { + enum trajectoire_type_t type; + struct point_xy_t p1, p2, p3, p4; + 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); +struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse); +float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse); +float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm); +double distance_points(struct point_xy_t point, struct point_xy_t point_old); +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_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_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); +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_bezier.c b/Trajectoire_bezier.c new file mode 100644 index 0000000..5c4aeab --- /dev/null +++ b/Trajectoire_bezier.c @@ -0,0 +1,35 @@ +#include "Trajectoire.h" +#include "Trajectoire_bezier.h" + + +void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){ + struct point_xy_t point, point_old; + float nb_pas=500; + + trajectoire->longueur=0; + point_old = trajectoire->p1; + + for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){ + point = Trajectoire_bezier_get_point(trajectoire, abscisse); + trajectoire->longueur += distance_points(point, point_old); + point_old = point; + } +} + + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){ + struct point_xy_t point; + point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.x * abscisse * abscisse * abscisse; + + point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + + 3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + + (double) trajectoire->p4.y * abscisse * abscisse * abscisse; + + return point; +} \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h new file mode 100644 index 0000000..5930684 --- /dev/null +++ b/Trajectoire_bezier.h @@ -0,0 +1,5 @@ +#include "Trajectoire.h" + + +void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse); \ No newline at end of file diff --git a/Trajectoire_circulaire.c b/Trajectoire_circulaire.c new file mode 100644 index 0000000..f104a40 --- /dev/null +++ b/Trajectoire_circulaire.c @@ -0,0 +1,26 @@ +#include "math.h" +#include "Trajectoire.h" + + +void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){ + float distance_angulaire; + if(trajectoire->angle_debut_rad > trajectoire->angle_fin_rad){ + distance_angulaire = trajectoire->angle_debut_rad - trajectoire->angle_fin_rad; + }else{ + distance_angulaire = trajectoire->angle_fin_rad - trajectoire->angle_debut_rad; + } + trajectoire->longueur = trajectoire->rayon * distance_angulaire; +} + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){ + struct point_xy_t point; + float angle_rad; + + angle_rad = (float) trajectoire->angle_debut_rad * (1-abscisse) + (float) trajectoire->angle_fin_rad * abscisse; + point.x = trajectoire->p1.x + cos(angle_rad) * trajectoire->rayon; + point.y = trajectoire->p1.y + sin(angle_rad) * trajectoire->rayon; + + return point; +} diff --git a/Trajectoire_circulaire.h b/Trajectoire_circulaire.h new file mode 100644 index 0000000..6229d88 --- /dev/null +++ b/Trajectoire_circulaire.h @@ -0,0 +1,4 @@ +#include "math.h" + +void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement); 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); diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c new file mode 100644 index 0000000..a2fa2a8 --- /dev/null +++ b/Trajectoire_droite.c @@ -0,0 +1,17 @@ +#include "Trajectoire.h" + + +void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire){ + trajectoire->longueur = distance_points(trajectoire->p1, trajectoire->p2); +} + +/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse +/// @param abscisse : compris entre 0 et 1 +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){ + struct point_xy_t point; + + point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse; + point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse; + + return point; +} \ No newline at end of file diff --git a/Trajectoire_droite.h b/Trajectoire_droite.h new file mode 100644 index 0000000..e4ed24c --- /dev/null +++ b/Trajectoire_droite.h @@ -0,0 +1,4 @@ +#include "Trajectoire.h" + +void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c new file mode 100644 index 0000000..ddd111f --- /dev/null +++ b/Trajet.c @@ -0,0 +1,219 @@ +#include +#include "Geometrie.h" +#include "Trajectoire.h" +#include "Trajet.h" +#include "Asser_Position.h" +#include "Asser_Moteurs.h" +#include "Temps.h" + +float Trajet_calcul_vitesse(float temps_s); +int Trajet_terminee(float abscisse); + +float abscisse; // Position entre 0 et 1 sur la trajectoire +float position_mm; // Position en mm sur la trajectoire +float vitesse_mm_s; +float vitesse_max_trajet_mm_s; +float acceleration_mm_ss; +const float acceleration_mm_ss_obstacle = 500; +struct trajectoire_t trajet_trajectoire; +struct position_t position_consigne; + +float distance_obstacle_mm; +float distance_fin_trajectoire_mm; +const float distance_pas_obstacle = 2000; + +float vitesse_max_contrainte_obstacle; + +/// @brief Initialise le module Trajet. A appeler en phase d'initialisation +void Trajet_init(int id){ + Temps_init(); + AsserMoteur_Init(id); + abscisse = 0; + vitesse_mm_s = 0; + position_mm = 0; + Trajet_config(TRAJECT_CONFIG_STD); +} + +/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets +/// @param _vitesse_max_trajet_mm_s +/// @param _acceleration_mm_ss +void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){ + vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s; + acceleration_mm_ss = _acceleration_mm_ss; +} + +void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){ + abscisse = 0; + vitesse_mm_s = 0; + position_mm = 0; + trajet_trajectoire = trajectoire; + Trajet_set_obstacle_mm(DISTANCE_INVALIDE); +} + +/// @brief Avance la consigne de position sur la trajectoire +/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde +/// @return TRAJET_EN_COURS ou TRAJET_TERMINE +struct point_xyo_t point; +enum etat_trajet_t Trajet_avance(float pas_de_temps_s){ + float distance_mm; + enum etat_trajet_t trajet_etat = TRAJET_EN_COURS; + + struct position_t position; + + // Calcul de la vitesse + vitesse_mm_s = Trajet_calcul_vitesse(pas_de_temps_s); + + // Calcul de l'avancement en mm + distance_mm = vitesse_mm_s * pas_de_temps_s; + position_mm += distance_mm; + + // Calcul de l'abscisse sur la trajectoire + abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm); + //set_debug_varf(abscisse); + + // Obtention du point consigne + point = Trajectoire_get_point(&trajet_trajectoire, abscisse); + + position.x_mm = point.point_xy.x; + position.y_mm = point.point_xy.y; + + position_consigne=position; + Asser_Position(position); + + if(Trajet_terminee(abscisse)){ + Asser_Position_set_Pos_Maintien(position); + trajet_etat = TRAJET_TERMINE; + } + return trajet_etat; + +} + +void Trajet_stop(float pas_de_temps_s){ + vitesse_mm_s = 0; + Trajet_avance(pas_de_temps_s); +} + +/// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier +/// où les approximations font que l'abscisse peut ne pas atteindre 1. +/// @param abscisse : abscisse sur la trajectoire +/// @return 1 si le trajet est terminé, 0 sinon +int Trajet_terminee(float abscisse){ + /*if(abscisse >= 0.99 ){ + return 1; + }*/ + + if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){ + if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){ + return 1; + } + }else{ + if(abscisse >= 0.99 ){ + return 1; + } + } + return 0; +} + +/// @brief Envoie la consigne de position calculée par le module trajet. Principalement pour le débug/réglage asservissement. +struct position_t Trajet_get_consigne(){ + return position_consigne; +} + +/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire +/// @param pas_de_temps_s : temps écoulé en ms +/// @return vitesse déterminée en m/s +float Trajet_calcul_vitesse(float pas_de_temps_s){ + float vitesse_max_contrainte; + float distance_contrainte,distance_contrainte_obstacle; + float vitesse; + // Calcul de la vitesse avec acceleration + vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s; + + // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s) + // https://poivron-robotique.fr/Consigne-de-vitesse.html + distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm; + distance_fin_trajectoire_mm=distance_contrainte; + // En cas de dépassement, on veut garder la contrainte, pour l'instant + if(distance_contrainte > 0){ + vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte); + }else{ + vitesse_max_contrainte = 0; + } + + distance_contrainte_obstacle = Trajet_get_obstacle_mm(); + if(distance_contrainte_obstacle != DISTANCE_INVALIDE){ + vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle); + if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){ + vitesse_max_contrainte = vitesse_max_contrainte_obstacle; + } + }/* + if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){ + vitesse = 0; + }*/ + + + // Selection de la vitesse la plus faible + if(vitesse > vitesse_max_contrainte){ + vitesse = vitesse_max_contrainte; + } + if(vitesse > vitesse_max_trajet_mm_s){ + vitesse = vitesse_max_trajet_mm_s; + } + return vitesse; +} + + +float Trajet_get_obstacle_mm(void){ + return distance_obstacle_mm; +} + +void Trajet_set_obstacle_mm(float distance_mm){ + distance_obstacle_mm = distance_mm; +} + + +/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain +/// @return angle en radian. +float Trajet_get_orientation_avance(){ + struct point_xyo_t point, point_suivant; + float avance_abscisse = 0.01; + float angle; + + if(abscisse >= 1){ + return 0; + } + if(abscisse + avance_abscisse >= 1){ + avance_abscisse = 1 - abscisse; + } + + point = Trajectoire_get_point(&trajet_trajectoire, abscisse); + point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse); + + angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x); + 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; +} + +/// @brief Indique si le robot est bloqué sur le trajet +/// @return 0 si le robot n'est pas bloqué, 1 s'il est bloqué +uint32_t Trajet_get_bloque(){ + if(Trajet_get_obstacle_mm() == DISTANCE_INVALIDE){ + return 0; + } + if (vitesse_max_contrainte_obstacle == 0){ + return 1; + } + return 0; +} \ No newline at end of file diff --git a/Trajet.h b/Trajet.h new file mode 100644 index 0000000..a07fcf2 --- /dev/null +++ b/Trajet.h @@ -0,0 +1,37 @@ +#include "pico/stdlib.h" +#include "Trajectoire.h" + +#ifndef TRAJET_H +#define TRAJET_H + +enum etat_trajet_t{ + TRAJET_EN_COURS, + TRAJET_TERMINE +}; + +// Vitesse et acceleration pour translation pure (en mm/s et mm/s²) +#define TRAJECT_CONFIG_RAPIDE 300, 1200 +#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200 +// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²) +#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500 +// Vitesse et acceleration - standard (en mm/s et mm/s²) +#define TRAJECT_CONFIG_STD 300, 300 +// Vitesse et acceleration pour une rotation (rad/s et rad/s²) +#define TRAJECT_CONFIG_ROTATION_PURE 2, 2 + +extern const float distance_pas_obstacle; + +void Trajet_init(int); +void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss); +void Trajet_debut_trajectoire(struct trajectoire_t trajectoire); +enum etat_trajet_t Trajet_avance(float temps_s); +struct position_t Trajet_get_consigne(void); +float Trajet_get_obstacle_mm(void); +void Trajet_set_obstacle_mm(float distance_mm); +void Trajet_stop(float); +float Trajet_get_orientation_avance(void); +float Trajet_get_abscisse(); +uint32_t Trajet_get_bloque(); +void Trajet_inverse(); + +#endif