diff --git a/Asser_Position.c b/Asser_Position.c index 55c43f2..45f235a 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -5,6 +5,8 @@ #define GAIN_P_POSITION 100 #define GAIN_P_ORIENTATION 20 +struct position_t position_maintien; + /// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot /// C'est à la consigne d'être défini avant pour être atteignable. /// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms); @@ -38,4 +40,12 @@ void Asser_Position(struct position_t position_consigne){ commande_vitesse(vitesse_robot_x_mm_s, vitesse_robot_y_mm_s, rotation_radian_s); +} + +void Asser_Position_set_Pos_Maintien(struct position_t position){ + position_maintien=position; +} + +void Asser_Position_maintien(){ + Asser_Position(position_maintien); } \ No newline at end of file diff --git a/Asser_Position.h b/Asser_Position.h index 6451fd4..d728db3 100644 --- a/Asser_Position.h +++ b/Asser_Position.h @@ -1,2 +1,4 @@ #include "Geometrie.h" -void Asser_Position(struct position_t position_consigne); \ No newline at end of file +void Asser_Position(struct position_t position_consigne); +void Asser_Position_set_Pos_Maintien(struct position_t position); +void Asser_Position_maintien(); diff --git a/Trajet.c b/Trajet.c index cc98d2b..e8b2010 100644 --- a/Trajet.c +++ b/Trajet.c @@ -67,6 +67,7 @@ enum etat_trajet_t Trajet_avance(double pas_de_temps_s){ Asser_Position(position); if(Trajet_terminee(abscisse)){ + Asser_Position_set_Pos_Maintien(position); trajet_etat = TRAJET_TERMINE; } return trajet_etat;