Initialisation du dépôt

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<MOTEUR_B+1; moteur++ ){
+        float erreur; // Erreur entre la consigne et la vitesse actuelle
+        float commande_P; // Terme proportionnel
+        float commande;
+        
+        // Calcul de l'erreur
+        erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
+
+        // Calcul du terme propotionnel
+        commande_P = erreur * ASSERMOTEUR_GAIN_P;
+
+        // Calcul du terme integral
+        commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
+
+        commande = commande_P + commande_I[moteur];
+
+        //Saturation de la commande
+        if(commande > 32760) {commande = 32760;}
+        if(commande < -32760) {commande = -32760;}
+
+        Moteur_SetVitesse(moteur, commande);
+
+    }
+}

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);

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;
+}

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();

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
+
+)

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);
+}

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();

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;
+}

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);

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);
+}

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);

QEI.c

@@ -0,0 +1,114 @@
+#include "config_robot.h"
+#include <stdio.h>
+#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;
+}

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);

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

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;  
+}

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);

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 <NB_MAX_TRAJECTOIRE ; index++){
+        trajectoire->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));
+    
+}

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

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;
+}

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);

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;
+}

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);

Trajectoire_composees.c

@@ -0,0 +1,24 @@
+#include <math.h>
+#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 ; i<trajectoire_composee->nb_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);
+}
+

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);

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;
+}

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);

Trajet.c

@@ -0,0 +1,219 @@
+#include <math.h>
+#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;
+}

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