Gestion des déplacements suivants des trajectoires avec un contrôle en vitesse.

Asser_Position.h

@@ -1 +1,2 @@
+#include "Geometrie.h"
 void Asser_Position(struct position_t position_consigne);

CMakeLists.txt

@@ -20,6 +20,11 @@ gyro_ADXRS453.c
 Localisation.c
 Moteurs.c
 Temps.c
+Trajet.c
+Trajectoire.c
+Trajectoire_bezier.c
+Trajectoire_circulaire.c
+Trajectoire_droite.c
 Servomoteur.c
 spi_nb.c)
 

Geometrie.h

@@ -0,0 +1,9 @@
+#ifndef GEOMETRIE_H
+#define GEOMETRIE_H
+
+struct position_t{
+    double x_mm, y_mm;
+    double angle_radian;
+};
+
+#endif

Localisation.h

@@ -1,7 +1,4 @@
-struct position_t{
-    double x_mm, y_mm;
-    double angle_radian;
-};
+#include "Geometrie.h"
 
 struct position_t Localisation_get(void);
 void Localisation_gestion();

Trajectoire.c

@@ -0,0 +1,117 @@
+#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.001
+
+
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon){
+    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;
+}
+
+void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y){
+    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;
+}
+
+void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y){
+    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;
+}
+
+/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
+/// @param trajectoire 
+/// @return Longueur de la trajectoire
+double 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_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
+    switch(trajectoire->type){
+        case TRAJECTOIRE_DROITE:
+            return Trajectoire_droite_get_point(trajectoire, abscisse);
+            break;
+
+        case TRAJECTOIRE_CIRCULAIRE:
+            return Trajectoire_circulaire_get_point(trajectoire, abscisse);
+            break;
+        
+        case TRAJECTOIRE_BEZIER:
+            return Trajectoire_bezier_get_point(trajectoire, abscisse);
+            break;
+    }
+    
+}
+
+/// @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
+double 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 et les trajectoires circulaires
+    delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire);
+    delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) );
+
+    // 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), Trajectoire_get_point(trajectoire, abscisse + delta_abscisse) );
+        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,30 @@
+#ifndef TRAJECTOIRE_H
+#define TRAJECTOIRE_H
+
+enum trajectoire_type_t{
+    TRAJECTOIRE_DROITE,
+    TRAJECTOIRE_CIRCULAIRE,
+    TRAJECTOIRE_BEZIER
+};
+
+struct point_xy_t{
+    double x, y;
+};
+
+struct trajectoire_t {
+    enum trajectoire_type_t type;
+    struct point_xy_t p1, p2, p3, p4;
+    double orientation_debut, orientation_fin;
+    double rayon, angle_debut_degre, angle_fin_degre;
+    double longueur;
+};
+
+double Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
+struct point_xy_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
+double 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, double centre_x, double centre_y, double angle_debut_degre, double angle_fin_degre, double rayon);
+void Trajectoire_droite(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y);
+void Trajectoire_bezier(struct trajectoire_t * trajectoire, double p1_x, double p1_y, double p2_x, double p2_y, double p3_x, double p3_y, double p4_x, double p4_y);
+
+#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;
+    double nb_pas=500;
+
+    trajectoire->longueur=0;
+    point_old = trajectoire->p1;
+
+    for(double 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){
+    double 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, double abscisse){
+    struct point_xy_t point;
+    double angle_degre;
+
+    angle_degre = (double) trajectoire->angle_debut_degre * (1-abscisse) + (double) 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, double 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, double abscisse){
+    struct point_xy_t point;
+
+    point.x = (double) trajectoire->p1.x * (1. - abscisse) + (double) trajectoire->p2.x * abscisse;
+    point.y = (double) trajectoire->p1.y * (1. - abscisse) + (double) 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, double abscisse);

Trajet.c

@@ -0,0 +1,94 @@
+#include <math.h>
+#include "Geometrie.h"
+#include "Trajectoire.h"
+#include "Asser_Position.h"
+
+#define VITESSE_MAX_MM_S 1000
+
+double Trajet_calcul_vitesse(double temps_s);
+
+double abscisse;
+double position_mm; // Position en mm sur la trajectoire
+double vitesse_mm_s;
+double acceleration_mm_ss;
+struct trajectoire_t trajet_trajectoire;
+struct position_t position_consigne;
+
+void Trajet_init(){
+    abscisse = 0;
+    vitesse_mm_s = 0;
+    acceleration_mm_ss = 300;
+    position_mm = 0;
+}
+
+void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
+    abscisse = 0;
+    vitesse_mm_s = 0;
+    acceleration_mm_ss = 1500;
+    position_mm = 0;
+    trajet_trajectoire = trajectoire;
+}
+
+void Trajet_avance(double pas_de_temps_s){
+    double distance_mm, orientation_radian;
+    struct point_xy_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);
+    
+    // Obtention du point consigne
+    point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
+
+    // Obtention de l'orientation consigne
+    orientation_radian = 0; // TODO
+
+    position.x_mm = point.x;
+    position.y_mm = point.y;
+    position.angle_radian = orientation_radian;
+
+    position_consigne=position;
+    Asser_Position(position);
+
+}
+
+struct position_t Trajet_get_consigne(){
+    return position_consigne;
+}
+
+/// @brief Calcule la vitesse à partir de l'accelé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
+double Trajet_calcul_vitesse(double pas_de_temps_s){
+    double vitesse_max_contrainte;
+    double distance_contrainte;
+    double 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;
+    // En cas de dépassement, on veut garder la contrainte, pour l'instant
+    if(distance_contrainte > 0){
+        vitesse_max_contrainte = sqrt(2 * acceleration_mm_ss * distance_contrainte);
+    }else{
+        vitesse_max_contrainte = 0;
+    }
+
+    // Selection de la vitesse la plus faible
+    if(vitesse > vitesse_max_contrainte){
+        vitesse = vitesse_max_contrainte;
+    }
+    if(vitesse > VITESSE_MAX_MM_S){
+        vitesse = VITESSE_MAX_MM_S;
+    }
+    return vitesse;
+}

Trajet.h

@@ -0,0 +1,5 @@
+
+void Trajet_init();
+void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
+void Trajet_avance(double temps_s);
+struct position_t Trajet_get_consigne(void);

test.c

@@ -6,16 +6,17 @@
 #include "math.h"
 
 #include "gyro.h"
-#include "Temps.h"
-#include "spi_nb.h"
-#include "Servomoteur.h"
+#include "Asser_Moteurs.h"
+#include "Asser_Position.h"
+#include "Commande_vitesse.h"
+#include "Localisation.h"
 #include "Moteurs.h"
 #include "QEI.h"
-#include "Asser_Moteurs.h"
-#include "Localisation.h"
-#include "Commande_vitesse.h"
-#include "Asser_Position.h"
-
+#include "Servomoteur.h"
+#include "spi_nb.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
 
 const uint LED_PIN = 25;
 const uint LED_PIN_ROUGE = 28;
@@ -38,6 +39,7 @@ int test_cde_vitesse_rectangle();
 int test_cde_vitesse_cercle();
 int test_asser_position_avance();
 int test_asser_position_avance_et_tourne();
+int test_trajectoire();
 void affiche_localisation();
 
 int main() {
@@ -148,6 +150,7 @@ int mode_test(){
     printf("I - Asser Position - avance et tourne\n");
     printf("M - pour les moteurs\n");
     printf("L - pour la localisation\n");
+    printf("T - Trajectoire\n");
     stdio_flush();
     int rep = getchar_timeout_us(TEST_TIMEOUT_US);
     stdio_flush();
@@ -204,9 +207,14 @@ int mode_test(){
         break;
     case 'L':
     case 'l':
-        /* code */
         while(test_localisation());
         break;
+
+    case 'T':
+    case 't':
+        while(test_trajectoire());
+        break;
+
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){
@@ -222,6 +230,64 @@ int mode_test(){
     
 }
 
+void test_trajectoire_printf(){
+    struct position_t _position;
+    while(1){
+        _position  = Trajet_get_consigne();
+        printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
+    }
+
+}
+
+int test_trajectoire(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    Trajet_init();
+    struct trajectoire_t trajectoire;
+    printf("Choix trajectoire :\n");
+    printf("B - Bezier\n");
+    printf("C - Circulaire\n");
+    printf("D - Droite\n");
+    do{
+         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    switch(lettre){
+        case 'b':
+        case 'B':
+            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0);
+            break;
+
+        case 'c':
+        case 'C':
+            Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250);
+            break;
+
+        case 'd':
+        case 'D':
+            Trajectoire_droite(&trajectoire, 0, 0, 0, 500);
+            break;
+        
+        default: return 0;
+    }
+
+    Trajet_debut_trajectoire(trajectoire);
+    multicore_launch_core1(test_trajectoire_printf);
+    do{
+        // Routines à 1 ms
+        QEI_update();
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+
+        Trajet_avance(_step_ms/1000.);
+        sleep_ms(_step_ms);
+        temps_ms += _step_ms;
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+    
+}
+
 int test_asser_position_avance_et_tourne(){
     int lettre, _step_ms = 1, temps_ms=0;
     struct position_t position_consigne;