diff --git a/Trajectoire.c b/Trajectoire.c index da57c99..8fe51c0 100644 --- a/Trajectoire.c +++ b/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; diff --git a/Trajectoire.h b/Trajectoire.h index c737929..1392ac2 100644 --- a/Trajectoire.h +++ b/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{ diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c index 5c4aeab..bc17b00 100644 --- a/Trajectoire_bezier.c +++ b/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) + - 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.x = trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) + + 3 * trajectoire->p3.x * abscisse * abscisse * (1-abscisse) + + 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; + point.y = trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + + 3 * trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) + + 3 * trajectoire->p3.y * abscisse * abscisse * (1-abscisse) + + trajectoire->p4.y * abscisse * abscisse * abscisse; return point; } \ No newline at end of file diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h index 5930684..dc6869f 100644 --- a/Trajectoire_bezier.h +++ b/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); \ No newline at end of file +void Trajectoire_bezier_rapide_get_longueur(struct trajectoire_t * trajectoire); +struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, float abscisse); diff --git a/Trajet.c b/Trajet.c index 4c10c26..b00fbfd 100644 --- a/Trajet.c +++ b/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 diff --git a/Trajet.h b/Trajet.h index c133dd1..c00d7a3 100644 --- a/Trajet.h +++ b/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);