Ajout des fonctions pour obtenir la vitesse des moteurs.

Changement de la vitesse max 1 m/s => 0,3 m/s

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;

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();

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}

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)

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);