trajectoires multiples fonctionnelles, testées avec la super star

.vscode/c_cpp_properties.json

@@ -2,6 +2,7 @@
     "env": {
         "myDefaultIncludePath": [
             "${workspaceFolder}",
+            "${workspaceFolder}/VL53L8CX_ULD_API/inc",
             "${workspaceFolder}/build",
             "${env:PICO_SDK_PATH}/src/**/include",
             "${env:PICO_SDK_PATH}/src/common/pico_base/include",

CMakeLists.txt

@@ -26,6 +26,7 @@ add_executable(Mon_Projet
   Temps.c
   Trajectoire_bezier.c
   Trajectoire_circulaire.c
+  Trajectoire_composees.c
   Trajectoire_droite.c
   Trajectoire.c
   Trajet.c

Strategie.c

@@ -5,46 +5,31 @@
 
 enum etat_action_t Strategie_super_star(uint32_t step_ms){
 	static enum{
-        SSS_TEST_TOURNE,
 		SSS_INIT,
-		SSS_AVANCE_1,
+		SSS_AVANCE,
-		SSS_TOURNE,
-		SSS_AVANCE_2,
 		SSS_DANCE
 	} etat_sss = SSS_INIT;
-    struct trajectoire_t trajectoire;
+    static struct trajectoire_t trajectoire_composee;
+    static struct trajectoire_t trajectoire1, trajectoire2, trajectoire3;
+
+
 
 	switch(etat_sss){
-        case SSS_TEST_TOURNE:
-            Localisation_set(1135, 1895, 0);
-            etat_sss = SSS_TOURNE;
-            break;
 		case SSS_INIT:
 			Localisation_set(45, 1895, 0);
-			etat_sss = SSS_AVANCE_1;
+            Trajectoire_droite(&trajectoire1, 45, 1895, 1135, 1895, 0, 0);
+            Trajectoire_circulaire(&trajectoire2, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
+            Trajectoire_droite(&trajectoire3, 1385, 1645, 1385, 1580, 0, 0);
+            Trajectoire_composee_init(&trajectoire_composee);
+            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire1);
+            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire2);
+            Trajectoire_composee_ajout(&trajectoire_composee, &trajectoire3);
+			etat_sss = SSS_AVANCE;
 			break;
 
-		case SSS_AVANCE_1:
+		case SSS_AVANCE:
-            Trajet_config(TRAJECT_CONFIG_STD);
+            Trajet_config(TRAJECT_CONFIG_RAPIDE);
-            Trajectoire_droite(&trajectoire, 45, 1895, 1135, 1895, 0, 0);
+            if(Strategie_parcourir_trajet(trajectoire_composee, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                etat_sss = SSS_TOURNE;
-            }
-            break;
-
-        case SSS_TOURNE:
-            //Trajet_config(100, 100);
-            Trajet_config(TRAJECT_CONFIG_STD);
-            Trajectoire_circulaire(&trajectoire, 1135, 1645, M_PI/2, 0, 250, 0, -M_PI/2);
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
-                etat_sss = SSS_AVANCE_2;
-            }
-            break;
-
-        case SSS_AVANCE_2:
-            Trajet_config(TRAJECT_CONFIG_STD);
-            Trajectoire_droite(&trajectoire, 1385, 1645, 1385, 1580, 0, 0);
-            if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){
                 etat_sss = SSS_DANCE;
             }
             break;

Trajectoire.c

@@ -2,15 +2,15 @@
 #include "Trajectoire_bezier.h"
 #include "Trajectoire_circulaire.h"
 #include "Trajectoire_droite.h"
+#include "Trajectoire_composees.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_rad, float angle_fin_rad, float rayon,
+void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_rad,
-                        float orientation_debut_rad, float orientation_fin_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;
@@ -34,8 +34,8 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
     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,
+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 orientation_debut_rad, float orientation_fin_rad){
+                        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;
@@ -50,6 +50,27 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1
     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;
@@ -97,6 +118,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
             case TRAJECTOIRE_BEZIER:
                 Trajectoire_bezier_get_longueur(trajectoire);
                 break;
+
+            case TRAJECTOIRE_COMPOSEE:
+                Trajectoire_composee_get_longueur(trajectoire);
+                break;
         }
     }
     return trajectoire->longueur;
@@ -123,6 +148,10 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
             point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
             break;
 
+        case TRAJECTOIRE_COMPOSEE:
+           point_xyo = Trajectoire_composee_get_point(trajectoire, abscisse);
+           break;
+
     }
     return point_xyo;    
 }

Trajectoire.h

@@ -1,10 +1,13 @@
 #ifndef TRAJECTOIRE_H
 #define TRAJECTOIRE_H
 
+#define NB_MAX_TRAJECTOIRE 10
+
 enum trajectoire_type_t{
     TRAJECTOIRE_DROITE,
     TRAJECTOIRE_CIRCULAIRE,
     TRAJECTOIRE_BEZIER,
+    TRAJECTOIRE_COMPOSEE
 };
 
 struct point_xy_t{
@@ -22,6 +25,9 @@ struct trajectoire_t {
     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);
@@ -35,6 +41,9 @@ void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1
 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_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);