Ajout de la fonction pour mettre à jour une trajectoire commencée

Trajectoire.c

@@ -44,6 +44,12 @@ void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1
     trajectoire->longueur = -1;
 }
 
+void Trajectoire_bezier_rapide(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){
+    trajectoire_bezier(trajectoire, p1_x, p1_y, p2_x, p2_y, p3_x, p3_y, p4_x, p4_y);
+    trajectoire->type = TRAJECTOIRE_BEZIER_RAPIDE;
+}
+
 /// @brief Initialise une trajectoire composée
 /// @param trajectoire : trajectoire à initialiser
 void Trajectoire_composee_init(struct trajectoire_t * trajectoire){
@@ -82,7 +88,7 @@ void Trajectoire_inverse(struct trajectoire_t * trajectoire){
         trajectoire->p2 = old_trajectoire.p1;
         return;
     }
-    if(trajectoire->type == TRAJECTOIRE_BEZIER){
+    if(trajectoire->type == TRAJECTOIRE_BEZIER || trajectoire->type == TRAJECTOIRE_BEZIER_RAPIDE){
         trajectoire->p1 = old_trajectoire.p4;
         trajectoire->p2 = old_trajectoire.p3;
         trajectoire->p3 = old_trajectoire.p2;
@@ -116,6 +122,10 @@ float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
             case TRAJECTOIRE_COMPOSEE:
                 Trajectoire_composee_get_longueur(trajectoire);
                 break;
+
+            case TRAJECTOIRE_BEZIER_RAPIDE:
+                Trajectoire_bezier_rapide_get_longueur(trajectoire);
+                break;
         }
     }
     return trajectoire->longueur;
@@ -136,6 +146,7 @@ struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, dou
             break;
         
         case TRAJECTOIRE_BEZIER:
+        case TRAJECTOIRE_BEZIER_RAPIDE:
             point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
             break;
 

Trajectoire.h

@@ -7,7 +7,8 @@ enum trajectoire_type_t{
     TRAJECTOIRE_DROITE=0,
     TRAJECTOIRE_CIRCULAIRE=1,
     TRAJECTOIRE_BEZIER=2,
-    TRAJECTOIRE_COMPOSEE=3
+    TRAJECTOIRE_COMPOSEE=3,
+    TRAJECTOIRE_BEZIER_RAPIDE=4
 };
 
 struct point_xy_t{

Trajectoire_bezier.c

@@ -16,20 +16,30 @@ void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){
     }
 }
 
+void Trajectoire_bezier_rapide_get_longueur(struct trajectoire_t * trajectoire){
+    struct point_xy_t point, point_old;
+    float nb_pas=500;
+
+    trajectoire->longueur=0;
+    point_old = trajectoire->p1;
+
+    return distance_points(trajectoire->p1, trajectoire->p4);
+}
+
 
 /// @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 Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse){
     struct point_xy_t point;
-    point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
+    point.x = trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
+        3 * trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
+        3 * trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
-            (double) trajectoire->p4.x * abscisse * abscisse * abscisse;
+            trajectoire->p4.x * abscisse * abscisse * abscisse;
                     
-    point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
+    point.y = trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
+        3 * trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
+        3 * trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
-            (double) trajectoire->p4.y * abscisse * abscisse * abscisse;
+            trajectoire->p4.y * abscisse * abscisse * abscisse;
 
     return point;
 }

Trajectoire_bezier.h

@@ -2,4 +2,5 @@
 
 
 void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire);
-struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse);
+void Trajectoire_bezier_rapide_get_longueur(struct trajectoire_t * trajectoire);
+struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse);

Trajet.c

@@ -44,6 +44,16 @@ void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
     Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
 }
 
+/// @brief Met à jour la trajectoire que le robot est en train de parcourir
+/// La trajectoire doit commencer au niveau du point consigne pour éviter les soucis.
+/// L'abscisse et la position sont remise à 0, pas la vitesse.
+/// @param trajectoire Nouvelle trajectoire
+void Trajet_continue_trajectoire(struct trajectoire_t trajectoire){
+    abscisse = 0;
+    position_mm = 0;
+    trajet_trajectoire = trajectoire;
+}
+
 /// @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

Trajet.h

@@ -26,6 +26,7 @@ extern const float distance_pas_obstacle;
 void Trajet_init(void);
 void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
 void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
+void Trajet_continue_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);