Correction code asservissement + réglage des gains

- Prise en compte du temps pour calculer la vitesse :-\
- Ajout saturation de la commande
- Initialisation de l'intégrateur à 0
- Réglage des gains P et I

Asser_Moteurs.c

@@ -1,7 +1,7 @@
 #include "QEI.h"
 #include "Moteurs.h"
 
-/*** C'est ici que ce fait la conversion en mm 
+/*** C'est ici que se fait la conversion en mm 
  * ***/
 
 // Roues 60 mm de diamètre, 188,5 mm de circonférence
@@ -13,12 +13,18 @@
 // Impulsion / mm : 42,44
 
 #define IMPULSION_PAR_MM (42.44f)
-#define ASSERMOTEUR_GAIN_P 20
-#define ASSERMOTEUR_GAIN_I 0.0f
+#define ASSERMOTEUR_GAIN_P 160
+#define ASSERMOTEUR_GAIN_I .80f
 
 double consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
 double commande_I[3]; // Terme integral
 
+void AsserMoteur_Init(){
+    for(unsigned int i =0; i< 3; i ++){
+        commande_I[i]=0;
+        consigne_mm_s[i]=0;
+    }
+}
 
 
 void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){
@@ -26,8 +32,9 @@ void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double _consigne_mm_s){
 
 }
 
-double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur){
+double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
     enum QEI_name_t qei;
+    double distance, temps;
     switch (moteur)
     {
     case MOTEUR_A: qei = QEI_A_NAME; break;
@@ -36,7 +43,10 @@ double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur){
     
     default: break;
     }
-    return (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
+    distance = (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
+    temps = step_ms / 1000.0;
+
+    return distance / temps;
 }
 
 void AsserMoteur_Gestion(int step_ms){
@@ -47,7 +57,7 @@ void AsserMoteur_Gestion(int step_ms){
         double commande;
         
         // Calcul de l'erreur
-        erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur);
+        erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
 
         // Calcul du terme propotionnel
         commande_P = erreur * ASSERMOTEUR_GAIN_P;
@@ -57,6 +67,10 @@ void AsserMoteur_Gestion(int step_ms){
 
         commande = commande_P + commande_I[moteur];
 
+        //Saturation de la commande
+        if(commande > 32760) {commande = 32760;}
+        if(commande < -32760) {commande = -32760;}
+
         Moteur_SetVitesse(moteur, commande);
 
     }

Asser_Moteurs.h

@@ -1,2 +1,6 @@
-void AsserMoteur_setConsigne_mm_s(enum moteur_t moteur, consigne_mm_s);
-AsserMoteur_Gestion(int step_ms);
+#include "Moteurs.h"
+
+void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, double consigne_mm_s);
+double AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
+void AsserMoteur_Gestion(int step_ms);
+void AsserMoteur_Init();

CMakeLists.txt

@@ -27,7 +27,7 @@ add_definitions(-DGYRO_ADXRS453)
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
 pico_add_extra_outputs(test)
-target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs hardware_pio)
+target_link_libraries(test pico_stdlib hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore)
 
 add_custom_target(Flash
     DEPENDS test

Moteurs.h

@@ -1,10 +1,13 @@
 #include "pico/stdlib.h"
 
+#ifndef MOTEURS_H
+#define MOTEURS_H
 enum t_moteur {
     MOTEUR_A=0,
     MOTEUR_B=1,
     MOTEUR_C=2
 };
+#endif
 
 void Moteur_Init(void);
 void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse );

test.c

@@ -1,4 +1,5 @@
 #include <stdio.h>
+#include "pico/multicore.h"
 #include "pico/stdlib.h"
 #include "hardware/gpio.h"
 #include "pico/binary_info.h"
@@ -9,6 +10,7 @@
 #include "Servomoteur.h"
 #include "Moteurs.h"
 #include "QEI.h"
+#include "Asser_Moteurs.h"
 
 const uint LED_PIN = 25;
 const uint LED_PIN_ROUGE = 28;
@@ -22,6 +24,7 @@ int mode_test();
 int test_moteurs();
 int test_QIE();
 int test_vitesse_moteur(enum t_moteur moteur);
+int test_asser_moteur();
 
 int main() {
     bi_decl(bi_program_description("This is a test binary."));
@@ -55,6 +58,7 @@ int main() {
     Temps_init();
     Moteur_Init();
     QEI_init();
+    AsserMoteur_Init();
 
     while(mode_test());
 
@@ -125,6 +129,10 @@ int mode_test(){
     stdio_flush();
     switch (rep)
     {
+    case 'a':
+    case 'A':
+        while(test_asser_moteur());
+        break;
     case 'C':
     case 'c':
         while(test_QIE());
@@ -149,12 +157,43 @@ int mode_test(){
     
 }
 
+
+void test_asser_moteur_printf(){
+    int _step_ms = 1;
+    while(1){
+        printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+            AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        //sleep_ms(5);
+    }
+}
+
+int test_asser_moteur(){
+    int lettre;
+    int _step_ms = 1;
+    printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 500);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 500);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 500);
+    multicore_launch_core1(test_asser_moteur_printf);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        //printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
+        //printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+        //    AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    multicore_reset_core1();
+    return 0;
+}
+
 int test_QIE(){
     int lettre;
     printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
     do{
         QEI_update();
-        printf("Codeur a : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
+        printf("Codeur A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
         sleep_ms(100);
 
         lettre = getchar_timeout_us(0);