From 324646d19933992c6b4553c29728f8a057144f7c Mon Sep 17 00:00:00 2001 From: Samuel Date: Mon, 28 Nov 2022 20:49:55 +0100 Subject: [PATCH] =?UTF-8?q?Correction=20code=20asservissement=20+=20r?= =?UTF-8?q?=C3=A9glage=20des=20gains?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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 | 26 ++++++++++++++++++++------ Asser_Moteurs.h | 8 ++++++-- CMakeLists.txt | 2 +- Moteurs.h | 3 +++ test.c | 41 ++++++++++++++++++++++++++++++++++++++++- 5 files changed, 70 insertions(+), 10 deletions(-) diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c index ae8bcca..e591d60 100644 --- a/Asser_Moteurs.c +++ b/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); } diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h index 6cb89e4..7c65545 100644 --- a/Asser_Moteurs.h +++ b/Asser_Moteurs.h @@ -1,2 +1,6 @@ -void AsserMoteur_setConsigne_mm_s(enum moteur_t moteur, consigne_mm_s); -AsserMoteur_Gestion(int step_ms); \ No newline at end of file +#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(); \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index bbcf2cf..0005cf2 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/Moteurs.h b/Moteurs.h index a44b080..7af1ea1 100644 --- a/Moteurs.h +++ b/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 ); diff --git a/test.c b/test.c index 15a8cb6..dc70132 100644 --- a/test.c +++ b/test.c @@ -1,4 +1,5 @@ #include +#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);