From ce8d5cff90a3222de7dd505606f1fe59ac7db2fc Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 29 Apr 2024 22:58:59 +0200 Subject: [PATCH] Premier deplacement acceptable --- Asser_Position.c | 10 ++++++++-- QEI.c | 3 ++- main.c | 12 +++++++++--- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/Asser_Position.c b/Asser_Position.c index df20315..aca2a44 100644 --- a/Asser_Position.c +++ b/Asser_Position.c @@ -2,8 +2,8 @@ #include "Commande_vitesse.h" #include "math.h" -#define GAIN_P_POSITION 10 -#define GAIN_P_ORIENTATION 100 +#define GAIN_P_POSITION 1 +#define GAIN_P_ORIENTATION 10 struct position_t position_maintien; @@ -24,6 +24,12 @@ void Asser_Position(struct position_t position_consigne){ delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm; delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm); delta_orientation_radian = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian; + delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian); + /*printf(">delta_orientation_radian:%.2f\n>angle_opti:%.2f\n>actuel:%.2f\n",delta_orientation_radian, + Geometrie_get_angle_optimal(atan2f(delta_y_mm, delta_x_mm), position_actuelle.angle_radian), + position_actuelle.angle_radian); + printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm); + printf(">con_x:%.2f\n>con_y:%.2f\n", position_consigne.x_mm, position_consigne.y_mm);*/ // Asservissement avance_mm_s = delta_avance_mm * GAIN_P_POSITION; diff --git a/QEI.c b/QEI.c index 149d115..4d8881f 100644 --- a/QEI.c +++ b/QEI.c @@ -17,7 +17,8 @@ // Nombre d'impulsions par tour de roue : 8000 // Impulsion / mm : 42,44 -#define IMPULSION_PAR_MM (12.45f) +//#define IMPULSION_PAR_MM (12.45f) +#define IMPULSION_PAR_MM (16.45f) struct QEI_t QEI_A, QEI_B; diff --git a/main.c b/main.c index 5bd8adc..3bd2b0b 100644 --- a/main.c +++ b/main.c @@ -41,7 +41,7 @@ void main(void) stdio_init_all(); Temps_init(); - tension_batterie_init(); + //tension_batterie_init(); identifiant_init(); Localisation_init(); Trajet_init(); @@ -57,14 +57,19 @@ void main(void) multicore_launch_core1(affichage); + Localisation_set(1122, 2000-63, M_PI); struct trajectoire_t trajectoire; - Trajectoire_droite(&trajectoire, 0, 0, 500, 0, 0, 0); + Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, + 606, 2000-590, 225, 2000-225, M_PI, M_PI); Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); Trajet_debut_trajectoire(trajectoire); + + + enum etat_trajet_t etat_trajet=TRAJET_EN_COURS; - sleep_ms(3000); + sleep_ms(8000); while(1){ @@ -96,6 +101,7 @@ void affichage(void){ printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/ printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian); printf(">abscisse:%f\n",abscisse); + printf(">id:%d\n", identifiant_lire()); sleep_ms(100); } }