From a325237c75d1c5cbbe48eb599842b2bf28a694c6 Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 1 Mar 2023 20:51:04 +0100 Subject: [PATCH] Ajout des fonctions pour obtenir la vitesse des moteurs. Changement de la vitesse max 1 m/s => 0,3 m/s --- Asser_Moteurs.c | 8 ++++++++ Asser_Moteurs.h | 1 + .../teleplot_layout_2023-1-20_21-29.json | 1 + Test.c | 19 ++++++++++++------- Trajet.c | 2 +- 5 files changed, 23 insertions(+), 8 deletions(-) create mode 100644 Teleplot_Layout/teleplot_layout_2023-1-20_21-29.json diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index 3df6325..6e44ea0 100644 --- a/Asser_Moteurs.c +++ b/Asser_Moteurs.c @@ -35,6 +35,14 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){ } +/// @brief Envoie la consigne du moteur +/// @param moteur : Moteur à asservir +double AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ + return consigne_mm_s[moteur]; +} + + + double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ enum QEI_name_t qei; double distance, temps; diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h index 7c65545..86e633d 100644 --- a/Asser_Moteurs.h +++ b/Asser_Moteurs.h @@ -1,6 +1,7 @@ #include "Moteurs.h" void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double consigne_mm_s); +double AsserMoteur_getConsigne_mm_s(enum t_moteur moteur); double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms); void AsserMoteur_Gestion(int step_ms); void AsserMoteur_Init(); \ No newline at end of file diff --git a/Teleplot_Layout/teleplot_layout_2023-1-20_21-29.json b/Teleplot_Layout/teleplot_layout_2023-1-20_21-29.json new file mode 100644 index 0000000..6726f55 --- /dev/null +++ b/Teleplot_Layout/teleplot_layout_2023-1-20_21-29.json @@ -0,0 +1 @@ +{"widgets":[{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"X","sourceNames":["X"],"formula":"","options":{"_serie":"X","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"Consigne_X","sourceNames":["Consigne_X"],"formula":"","options":{"_serie":"Consigne_X","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"Y","sourceNames":["Y"],"formula":"","options":{"_serie":"Y","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"Consigne_Y","sourceNames":["Consigne_Y"],"formula":"","options":{"_serie":"Consigne_Y","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"orientation","sourceNames":["orientation"],"formula":"","options":{"_serie":"orientation","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"Consigne_orientation","sourceNames":["Consigne_orientation"],"formula":"","options":{"_serie":"Consigne_orientation","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"Position","sourceNames":["Position"],"formula":"","options":{"_serie":"Position","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"Consigne_Position","sourceNames":["Consigne_Position"],"formula":"","options":{"_serie":"Consigne_Position","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"V_A","sourceNames":["V_A"],"formula":"","options":{"_serie":"V_A","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"V_consigne_A","sourceNames":["V_consigne_A"],"formula":"","options":{"_serie":"V_consigne_A","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"V_B","sourceNames":["V_B"],"formula":"","options":{"_serie":"V_B","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"V_consigne_B","sourceNames":["V_consigne_B"],"formula":"","options":{"_serie":"V_consigne_B","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]},{"type":"chart","gridPos":{"h":6,"w":6,"x":0,"y":0},"series":[{"name":"V_C","sourceNames":["V_C"],"formula":"","options":{"_serie":"V_C","stroke":"rgba(231,76,60,1)","fill":"rgba(231,76,60,0.1)"}},{"name":"V_consigne_C","sourceNames":["V_consigne_C"],"formula":"","options":{"_serie":"V_consigne_C","stroke":"rgba(52,152,219,1)","fill":"rgba(52,152,219,0.1)"}}]}],"viewDuration":15} \ No newline at end of file diff --git a/Test.c b/Test.c index 112b750..ba035fe 100644 --- a/Test.c +++ b/Test.c @@ -453,7 +453,9 @@ void test_trajectoire_teleplot(){ temps = time_us_32()/1000; printf(">X:%ld:%f\n>Y:%ld:%f\n>orientation:%ld:%f\n", temps, _position.x_mm, temps, _position.y_mm, temps, _position.angle_radian/M_PI*180); printf(">Consigne_X:%ld:%f\n>Consigne_Y:%ld:%f\n>Consigne_orientation:%ld:%f\n", temps, _consigne.x_mm, temps, _consigne.y_mm, temps, _consigne.angle_radian/M_PI*180); - printf(">Position:%f:%f|xy\n>Consigne_Position:%f:%f|xy\n", _position.x_mm, _position.y_mm, _consigne.x_mm, _consigne.y_mm); + printf(">Position:%f:%f:%ld|xy\n>Consigne_Position:%f:%f:%ld|xy\n", _position.x_mm, _position.y_mm, temps, _consigne.x_mm, _consigne.y_mm, temps); + printf(">V_A:%ld:%f\n>V_B:%ld:%f\n>V_C:%ld:%f\n", temps, QEI_get_mm(QEI_A_NAME), temps, QEI_get_mm(QEI_B_NAME), temps, QEI_get_mm(QEI_C_NAME)); + printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); } } @@ -480,7 +482,7 @@ int test_aller_retour(){ case 'c': case 'C': - Trajectoire_circulaire(&trajectoire, 0, 350, -90, 90, 350); + Trajectoire_circulaire(&trajectoire, 0, 1000, -90, 90, 1000); printf("Trajectoire circulaire\n"); break; @@ -492,9 +494,9 @@ int test_aller_retour(){ case 'e': case 'E': - Trajectoire_droite(&trajectoire, 0, 0, 0, 700); + Trajectoire_droite(&trajectoire, 0, 0, 0, 2500); trajectoire.orientation_debut_rad = 0; - trajectoire.orientation_fin_rad = -M_PI; + trajectoire.orientation_fin_rad = M_PI; printf("Trajectoire droite avec rotation\n"); break; @@ -510,8 +512,11 @@ int test_aller_retour(){ Trajet_debut_trajectoire(trajectoire); multicore_launch_core1(test_trajectoire_teleplot); + temps_ms = Temps_get_temps_ms(); do{ // Routines à 1 ms + while(temps_ms == Temps_get_temps_ms()); + temps_ms = Temps_get_temps_ms(); QEI_update(); Localisation_gestion(); @@ -526,14 +531,13 @@ int test_aller_retour(){ }else{ AsserMoteur_Gestion(_step_ms); } - sleep_ms(_step_ms); - temps_ms += _step_ms; lettre = getchar_timeout_us(0); //lettre = PICO_ERROR_TIMEOUT; }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); printf("Lettre : %d; %c\n", lettre, lettre); Moteur_Stop(); + multicore_reset_core1(); return 0; @@ -947,8 +951,9 @@ int test_vitesse_moteur(enum t_moteur moteur){ int vitesse_moteur; do{ - vitesse_moteur = getchar_timeout_us(TEST_TIMEOUT_US); + vitesse_moteur = getchar_timeout_us(0); stdio_flush(); + }while(vitesse_moteur == PICO_ERROR_TIMEOUT); switch (vitesse_moteur) diff --git a/Trajet.c b/Trajet.c index 28ffbdd..f4d3a1c 100644 --- a/Trajet.c +++ b/Trajet.c @@ -4,7 +4,7 @@ #include "Trajet.h" #include "Asser_Position.h" -#define VITESSE_MAX_MM_S 1000 +#define VITESSE_MAX_MM_S 300 double Trajet_calcul_vitesse(double temps_s);