Réglage de l'asservissement avec pile 9V

Asser_Position.c

@@ -2,8 +2,8 @@
 #include "Commande_vitesse.h"
 #include "math.h"
 
-#define GAIN_P_POSITION 50
-#define GAIN_P_ORIENTATION 500
+#define GAIN_P_POSITION 10
+#define GAIN_P_ORIENTATION 100
 
 struct position_t position_maintien;
 

CMakeLists.txt

@@ -19,6 +19,11 @@ add_executable(Mon_Projet
   Localisation.c
   main.c
   Temps.c
+  Trajectoire_bezier.c
+  Trajectoire_circulaire.c
+  Trajectoire_droite.c
+  Trajectoire.c
+  Trajet.c
   QEI.c
 )
 

Trajectoire.c

@@ -0,0 +1,167 @@
+#include "Trajectoire.h"
+#include "Trajectoire_bezier.h"
+#include "Trajectoire_circulaire.h"
+#include "Trajectoire_droite.h"
+
+#include "math.h"
+
+#define NB_MAX_TRAJECTOIRES 5
+#define PRECISION_ABSCISSE 0.001f
+
+
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, 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_degre = angle_debut_degre;
+    trajectoire->angle_fin_degre = angle_fin_degre;
+    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;
+}
+
+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_degre = old_trajectoire.angle_fin_degre;
+        trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
+        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;
+        }
+    }
+    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);
+            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
+            break;
+
+        case TRAJECTOIRE_CIRCULAIRE:
+            point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse);
+            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
+            break;
+        
+        case TRAJECTOIRE_BEZIER:
+            point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
+            point_xyo.orientation = Trajectoire_get_orientation_rad(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;
+    while(fabs(erreur_relative) > PRECISION_ABSCISSE){
+        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;
+    }
+    
+    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,40 @@
+#ifndef TRAJECTOIRE_H
+#define TRAJECTOIRE_H
+
+enum trajectoire_type_t{
+    TRAJECTOIRE_DROITE,
+    TRAJECTOIRE_CIRCULAIRE,
+    TRAJECTOIRE_BEZIER,
+};
+
+struct point_xy_t{
+    double x, y;
+};
+
+struct point_xyo_t{
+    struct point_xy_t point_xy;
+    float orientation;
+};
+
+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_degre, angle_fin_degre;
+    float longueur;
+};
+
+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_degre, float angle_fin_degre,
+                            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_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_degre > trajectoire->angle_fin_degre){
+        distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
+    }else{
+        distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre;
+    }
+    trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.;
+}
+
+/// @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_degre;
+
+    angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
+    point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
+    point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * 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_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,216 @@
+#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=500;
+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'initilisation
+void Trajet_init(){
+    Temps_init();
+    AsserMoteur_Init();
+    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
+enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
+    float distance_mm;
+    enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
+    struct point_xyo_t point;
+    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.angle_radian = point.orientation;
+
+    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(0);
+}
+
+/// @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;
+        }
+    }
+    
+
+    // 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,36 @@
+#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_AVANCE_DROIT 1000, 500
+// 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 et mm/s²)
+#define TRAJECT_CONFIG_STD 500, 500
+// 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();
+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

main.c

@@ -9,10 +9,12 @@
 #include "hardware/pwm.h"
 #include "Asser_Position.h"
 #include "Asser_Moteurs.h"
-#include "Localisation.h"
 #include "Commande_vitesse.h"
+#include "Localisation.h"
 #include "Moteurs.h"
 #include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
 #include <stdio.h>
 #include "QEI.h"
 
@@ -28,6 +30,9 @@ uint identifiant_lire(void);
 uint32_t step_ms=1;
 float distance1_mm=0, distance2_mm=0;
 
+// DEBUG
+extern float abscisse; 
+
 void main(void)
 {
 	int ledpower = 500;
@@ -35,11 +40,12 @@ void main(void)
 
 
     stdio_init_all();
-	AsserMoteur_Init();
 	Temps_init();
 	tension_batterie_init();
 	identifiant_init();
 	Localisation_init();
+	Trajet_init();
+
 	uint32_t temps_ms = Temps_get_temps_ms();
 	struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
 	float vitesse_mm_s=100;
@@ -51,7 +57,14 @@ void main(void)
 
 	
 	multicore_launch_core1(affichage);
-	
+
+	struct trajectoire_t trajectoire;
+	Trajectoire_droite(&trajectoire, 0, 0, 500, 0, 0, 0);
+	Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+	Trajet_debut_trajectoire(trajectoire);
+	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
+
+	sleep_ms(3000);
 
 	while(1){
 		
@@ -59,15 +72,14 @@ void main(void)
 			temps_ms = Temps_get_temps_ms();
 			if(temps_ms % step_ms == 0){
 				QEI_update();
-				position_robot.x_mm += temps_ms * vitesse_mm_s / 1000.;
-				position_robot.y_mm += temps_ms * vitesse_mm_s / 1000.;
-				Asser_Position(position_robot);
-				AsserMoteur_Gestion(step_ms);
 				Localisation_gestion();
-			}
-			if(temps_ms % 100 == 0){
-				identifiant_lire();
-				//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );	
+				
+				if(etat_trajet != TRAJET_TERMINE){
+					AsserMoteur_Gestion(step_ms);
+					etat_trajet = Trajet_avance((float)step_ms/1000.);
+				}else{
+					Moteur_Stop();
+				}
 			}
 		}
 
@@ -83,6 +95,7 @@ void affichage(void){
 		/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
 		printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
 		printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
+		printf(">abscisse:%f\n",abscisse);
 		sleep_ms(100);
 	}
 }