From 6162c6f412996b6e74fd08ea6970fc53a075ba9f Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 27 Apr 2024 21:21:41 +0200 Subject: [PATCH] Asservissement moteurs - fonctionnel ? --- Asser_Moteurs.c | 102 ++++++++++++++++++++++++++++++++++++ Asser_Moteurs.h | 8 +++ CMakeLists.txt | 3 ++ Moteurs.c | 98 +++++++++++++++++++++++++++++++++++ Moteurs.h | 14 +++++ QEI.c | 12 ++--- Temps.c | 24 +++++++++ Temps.h | 5 ++ main.c | 135 +++++++++++++----------------------------------- 9 files changed, 296 insertions(+), 105 deletions(-) create mode 100644 Asser_Moteurs.c create mode 100644 Asser_Moteurs.h create mode 100644 Moteurs.c create mode 100644 Moteurs.h create mode 100644 Temps.c create mode 100644 Temps.h diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c new file mode 100644 index 0000000..300a9e8 --- /dev/null +++ b/Asser_Moteurs.c @@ -0,0 +1,102 @@ +#include "QEI.h" +#include "Moteurs.h" +#include "Asser_Moteurs.h" + +/*** C'est ici que se fait la conversion en mm + * ***/ + +// Roues 60 mm de diamètre, 188,5 mm de circonférence +// Réduction Moteur 30:1 +// Réduction poulie 16:12 +// Nombre d'impulsions par tour moteur : 200 +// Nombre d'impulsions par tour réducteur : 6000 +// Nombre d'impulsions par tour de roue : 8000 +// Impulsion / mm : 42,44 + +#define IMPULSION_PAR_MM (1.f) +#define ASSERMOTEUR_GAIN_P 2.f +#define ASSERMOTEUR_GAIN_I 0.f + +float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) +float commande_I[3]; // Terme integral + +void AsserMoteur_Init(){ + QEI_init(); + Moteur_Init(); + for(unsigned int i =0; i< 2; i ++){ + commande_I[i]=0; + consigne_mm_s[i]=0; + } +} + +/// @brief Défini une consigne de vitesse pour le moteur indiqué. +/// @param moteur : Moteur à asservir +/// @param _consigne_mm_s : consigne de vitesse en mm/s +void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){ + consigne_mm_s[moteur] = _consigne_mm_s; + +} + +/// @brief Envoie la consigne du moteur +/// @param moteur : Moteur à asservir +float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){ + return consigne_mm_s[moteur]; +} + + + +float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){ + enum QEI_name_t qei; + float distance, temps; + switch (moteur) + { + case MOTEUR_A: qei = QEI_A_NAME; break; + case MOTEUR_B: qei = QEI_B_NAME; break; + default: break; + } + distance = QEI_get_mm(qei); + temps = step_ms / 1000.0f; + + return distance / temps; +} + +/// @brief Indique si le robot est à l'arrêt +/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses) +/// @return 1 si le robot est immobile, 0 s'il est en mouvement. +uint32_t AsserMoteur_RobotImmobile(int step_ms){ + const float seuil_vitesse_immobile_mm_s = 0.1; + if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s && + AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s ){ + return 1; + } + return 0; +} + +/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement +/// @param step_ms +void AsserMoteur_Gestion(int step_ms){ + // Pour chaque moteur + for(uint moteur=MOTEUR_A; moteur 32760) {commande = 32760;} + if(commande < -32760) {commande = -32760;} + + Moteur_SetVitesse(moteur, commande); + + } +} \ No newline at end of file diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h new file mode 100644 index 0000000..aad202f --- /dev/null +++ b/Asser_Moteurs.h @@ -0,0 +1,8 @@ +#include "Moteurs.h" + +uint32_t AsserMoteur_RobotImmobile(int step_ms); +void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s); +float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur); +float 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 3c41f48..4030050 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,10 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) pico_sdk_init() add_executable(Mon_Projet + Asser_Moteurs.c + Moteurs.c main.c + Temps.c QEI.c ) diff --git a/Moteurs.c b/Moteurs.c new file mode 100644 index 0000000..a3dc9a8 --- /dev/null +++ b/Moteurs.c @@ -0,0 +1,98 @@ +#include "hardware/pwm.h" +#include "Moteurs.h" + +#define MOTEUR_A 0 +#define MOTEUR_B 1 +#define MOTEUR_C 2 + +#define MOTEUR_A_VITESSE 6 +#define MOTEUR_B_VITESSE 8 +#define MOTEUR_C_VITESSE 10 + +#define MOTEUR_A_SENS 5 +#define MOTEUR_B_SENS 7 +#define MOTEUR_C_SENS 9 + +#define M1_SENS1 7 +#define M1_SENS2 13 +#define M1_VITESSE 27 //5B +#define M2_SENS1 10 +#define M2_SENS2 5 +#define M2_VITESSE 9 //4B + +uint slice_moteur_A, slice_moteur_B, slice_moteur_C; + +/// @brief Initialisation les entrées / sorties requises pour les moteurs +void Moteur_Init(){ + gpio_init(M1_SENS1); + gpio_init(M1_SENS2); + gpio_init(M2_SENS1); + gpio_init(M2_SENS2); + gpio_set_dir(M1_SENS1, GPIO_OUT); + gpio_set_dir(M1_SENS2, GPIO_OUT); + gpio_set_dir(M2_SENS1, GPIO_OUT); + gpio_set_dir(M2_SENS2, GPIO_OUT); + gpio_put(M1_SENS1, 0); + gpio_put(M1_SENS2, 0); + gpio_put(M2_SENS1, 0); + gpio_put(M2_SENS2, 0); + + gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM); + gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM); + pwm_set_wrap(5, (uint16_t)65535); + pwm_set_wrap(4, (uint16_t)65535); + pwm_set_chan_level(5, PWM_CHAN_B, 0); + pwm_set_chan_level(4, PWM_CHAN_B, 0); + pwm_set_enabled(5, true); + pwm_set_enabled(4, true); + + Moteur_SetVitesse(MOTEUR_A, 0); + Moteur_SetVitesse(MOTEUR_B, 0); +} + + +/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur +/// @param moteur : Moteur (voir enum t_moteur) +/// @param vitesse : Vitesse entre -32767 et 32767 +void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){ + uint16_t u_vitesse; + + // Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe) + if (vitesse < 0){ + u_vitesse = -vitesse; + }else{ + u_vitesse = vitesse; + } + u_vitesse = u_vitesse * 2; + + switch(moteur){ + case MOTEUR_A: + pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse); + if(vitesse > 0){ + gpio_put(M1_SENS1, 0); + gpio_put(M1_SENS2, 1); + }else{ + gpio_put(M1_SENS1, 1); + gpio_put(M1_SENS2, 0); + } + break; + + case MOTEUR_B: + pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse); + if(vitesse < 0){ + gpio_put(M2_SENS1, 0); + gpio_put(M2_SENS2, 1); + }else{ + gpio_put(M2_SENS1, 1); + gpio_put(M2_SENS2, 0); + } + break; + + } + +} + +void Moteur_Stop(void){ + Moteur_SetVitesse(MOTEUR_A, 0); + Moteur_SetVitesse(MOTEUR_B, 0); +} \ No newline at end of file diff --git a/Moteurs.h b/Moteurs.h new file mode 100644 index 0000000..1eed7da --- /dev/null +++ b/Moteurs.h @@ -0,0 +1,14 @@ +#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 ); +void Moteur_Stop(void); diff --git a/QEI.c b/QEI.c index 4a5e431..af9d384 100644 --- a/QEI.c +++ b/QEI.c @@ -17,9 +17,9 @@ // Nombre d'impulsions par tour de roue : 8000 // Impulsion / mm : 42,44 -#define IMPULSION_PAR_MM (95.4929658551372f) -#define ASSERMOTEUR_GAIN_P 160 -#define ASSERMOTEUR_GAIN_I .80f +#define IMPULSION_PAR_MM (1.f) +#define ASSERMOTEUR_GAIN_P 2 +#define ASSERMOTEUR_GAIN_I .0f struct QEI_t QEI_A, QEI_B; @@ -87,11 +87,11 @@ int QEI_get(enum QEI_name_t qei){ switch (qei) { case QEI_A_NAME: - return QEI_A.delta; + return -QEI_A.delta; break; case QEI_B_NAME: - return -QEI_B.delta; + return QEI_B.delta; break; default: @@ -103,5 +103,5 @@ int QEI_get(enum QEI_name_t qei){ /// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME) /// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update() float QEI_get_mm(enum QEI_name_t qei){ - return (float) QEI_get(qei) / (float)IMPULSION_PAR_MM; + return ((float) QEI_get(qei)) / (float)IMPULSION_PAR_MM; } \ No newline at end of file diff --git a/Temps.c b/Temps.c new file mode 100644 index 0000000..2674832 --- /dev/null +++ b/Temps.c @@ -0,0 +1,24 @@ +#include +#include "pico/stdlib.h" +#include "Temps.h" + +uint32_t temps_ms=0; +bool temps_est_init=false; +struct repeating_timer timer; + +bool Temps_increment(struct repeating_timer *t){ + temps_ms++; + return true; +} + +void Temps_init(void){ + if(!temps_est_init){ + temps_ms=0; + add_repeating_timer_ms(-1, Temps_increment, NULL, &timer); + temps_est_init = true; + } +} + +uint32_t Temps_get_temps_ms(void){ + return temps_ms; +} \ No newline at end of file diff --git a/Temps.h b/Temps.h new file mode 100644 index 0000000..0a575a3 --- /dev/null +++ b/Temps.h @@ -0,0 +1,5 @@ +#include "pico/stdlib.h" + +bool Temps_increment(struct repeating_timer *t); +void Temps_init(void); +uint32_t Temps_get_temps_ms(void); \ No newline at end of file diff --git a/main.c b/main.c index 6aee4c2..653ae26 100644 --- a/main.c +++ b/main.c @@ -4,7 +4,11 @@ * SPDX-License-Identifier: BSD-3-Clause */ #include "pico/stdlib.h" +#include "pico/multicore.h" #include "hardware/pwm.h" +#include "Asser_Moteurs.h" +#include "Moteurs.h" +#include "Temps.h" #include #include "QEI.h" @@ -16,119 +20,52 @@ #define M2_SENS2 5 #define M2_VITESSE 9 //4B -void Moteur_init(void); -void M1_forward(int speed); -void M1_backward(int speed); -void M1_speed(int speed); -void M2_forward(int speed); -void M2_backward(int speed); -void M2_speed(int speed); +void affichage(void); void main(void) { int ledpower = 500; + stdio_init_all(); - QEI_init(); - Moteur_init(); + AsserMoteur_Init(); + Temps_init(); + uint32_t temps_ms = Temps_get_temps_ms(); + uint32_t step_ms=1; gpio_init(LED1PIN); gpio_set_dir(LED1PIN, GPIO_OUT ); gpio_put(LED1PIN, 1); + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 6000); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, 6000); + + multicore_launch_core1(affichage); + Moteur_SetVitesse(MOTEUR_A, 16000); + Moteur_SetVitesse(MOTEUR_B, 16000); while(1){ - QEI_update(); + + if(temps_ms != Temps_get_temps_ms()){ + temps_ms = Temps_get_temps_ms(); + QEI_update(); + AsserMoteur_Gestion(step_ms); + if(temps_ms % 100 == 0){ + //printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) ); + } + } - printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) ); - M1_speed(1000); - M2_speed(1000); + } +} + +void affichage(void){ + while(1){ + + printf(">c1:%d\n>c1_mm:%f\n", QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME) ); + //printf(">c2:%d\n>c2_mm:%f\n", QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME) ); + printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1), AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1) ); + printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) ); sleep_ms(100); } -} - -void Moteur_init(void){ - - gpio_init(M1_SENS1); - gpio_init(M1_SENS2); - gpio_init(M2_SENS1); - gpio_init(M2_SENS2); - gpio_set_dir(M1_SENS1, GPIO_OUT); - gpio_set_dir(M1_SENS2, GPIO_OUT); - gpio_set_dir(M2_SENS1, GPIO_OUT); - gpio_set_dir(M2_SENS2, GPIO_OUT); - gpio_put(M1_SENS1, 0); - gpio_put(M1_SENS2, 0); - gpio_put(M2_SENS1, 0); - gpio_put(M2_SENS2, 0); - - gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM); - gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM); - pwm_set_wrap(5, 1000); - pwm_set_wrap(4, 1000); - pwm_set_chan_level(5, PWM_CHAN_B, 0); - pwm_set_chan_level(4, PWM_CHAN_B, 0); - pwm_set_enabled(5, true); - pwm_set_enabled(4, true); - -} - -void M1_forward(int speed) -{ - pwm_set_chan_level(5, PWM_CHAN_B, speed); - gpio_put(M1_SENS1, 1); - gpio_put(M1_SENS2, 0); -} - -// Set motor 1 speed backward -void M1_backward(int speed) -{ - pwm_set_chan_level(5, PWM_CHAN_B, speed); - gpio_put(M1_SENS1, 0); - gpio_put(M1_SENS2, 1); -} - -// Set motor 1 speed and direction (negative value : backward / positive value : forward) -void M1_speed(int speed) -{ - if(speed < 0) - { - speed = -speed; - M1_backward(speed); - } - else - { - M1_forward(speed); - } -} - -// Set motor 2 speed forward -void M2_forward(int speed) -{ - pwm_set_chan_level(4, PWM_CHAN_B, speed); - gpio_put(M2_SENS1, 0); - gpio_put(M2_SENS2, 1); -} - -// Set motor 2 speed backward -void M2_backward(int speed) -{ - pwm_set_chan_level(4, PWM_CHAN_B, speed); - gpio_put(M2_SENS1, 1); - gpio_put(M2_SENS2, 0); -} - -// Set motor 2 speed and direction (negative value : backward / positive value : forward) -void M2_speed(int speed) -{ - if(speed < 0) - { - speed = -speed; - M2_backward(speed); - } - else - { - M2_forward(speed); - } -} +} \ No newline at end of file