diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c new file mode 100644 index 0000000..5c826f6 --- /dev/null +++ b/Asser_Moteurs.c @@ -0,0 +1,79 @@ +#include "QEI.h" +#include "Moteurs.h" +#include "Asser_Moteurs.h" + +#define ASSERMOTEUR_GAIN_P 300000.f +#define ASSERMOTEUR_GAIN_I 30000.f +//#define ASSERMOTEUR_GAIN_I 0000.f + +float consigne_mm_s[3]; // Consigne de vitesse (en mm/s) +float commande_I[3]; // Terme integral + +void AsserMoteur_init(void){ + 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 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_set_commande(moteur, commande); + + } +} \ No newline at end of file diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h new file mode 100644 index 0000000..64be16e --- /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(void); \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 4dda132..cb22c1f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.13) include(pico_sdk_import.cmake) -project(PAMI_Cours_Codeurs C CXX ASM) +project(PAMI_Cours_Asser_Moteurs C CXX ASM) set(CMAKE_C_STNDARD 11) set(CMAKE_CXX_STANDARD 17) @@ -10,7 +10,8 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) pico_sdk_init() -add_executable(PAMI_Cours_Codeurs +add_executable(PAMI_Cours_Asser_Moteurs + Asser_Moteurs.c main.c Moteurs.c QEI.c @@ -18,11 +19,11 @@ add_executable(PAMI_Cours_Codeurs Temps.c ) -pico_generate_pio_header(PAMI_Cours_Codeurs ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) +pico_generate_pio_header(PAMI_Cours_Asser_Moteurs ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) -target_include_directories(PAMI_Cours_Codeurs PRIVATE ${CMAKE_CURRENT_LIST_DIR}) +target_include_directories(PAMI_Cours_Asser_Moteurs PRIVATE ${CMAKE_CURRENT_LIST_DIR}) -target_link_libraries(PAMI_Cours_Codeurs +target_link_libraries(PAMI_Cours_Asser_Moteurs hardware_uart hardware_pio hardware_pwm @@ -31,12 +32,12 @@ target_link_libraries(PAMI_Cours_Codeurs pico_cyw43_arch_lwip_poll ) -pico_enable_stdio_usb(PAMI_Cours_Codeurs 1) -pico_enable_stdio_uart(PAMI_Cours_Codeurs 1) +pico_enable_stdio_usb(PAMI_Cours_Asser_Moteurs 1) +pico_enable_stdio_uart(PAMI_Cours_Asser_Moteurs 1) -pico_add_extra_outputs(PAMI_Cours_Codeurs) +pico_add_extra_outputs(PAMI_Cours_Asser_Moteurs) add_custom_target(Flash - DEPENDS PAMI_Cours_Codeurs - COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Codeurs.uf2 + DEPENDS PAMI_Cours_Asser_Moteurs + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Asser_Moteurs.uf2 ) diff --git a/QEI.c b/QEI.c index 8e13bc0..df9cb44 100644 --- a/QEI.c +++ b/QEI.c @@ -7,7 +7,7 @@ // C'est ici que se fait la conversion en mm -#define IMPULSION_PAR_MM (1.f) +#define IMPULSION_PAR_MM (12.45f) float impulsion_par_mm; diff --git a/Readme.md b/Readme.md index 15b17aa..8edba9d 100644 --- a/Readme.md +++ b/Readme.md @@ -1,10 +1,12 @@ -De l’art de déplacer un robot avec classe - Lecture des codeurs -================================================================ +De l’art de déplacer un robot avec classe - Asservissement des moteurs. +======================================================================= Le but est de vous proposer une série d’articles vous présentant les arcanes du déplacement d’un robot, en fournissant à chaque étape une idée de démonstration et les principales pistes de débogage. Les articles sont disponibles ici : [Wiki Eurobot](https://www.eurobot.org/wiki/fr/informatics/de_l_art_de_deplacer_son_robot_avec_classe) -Lecture des codeurs +Asservissement des moteurs ------------------- -Ajout des fichiers pour gérer les codeurs. Le code principal fait évoluer la vitesse des moteurs progressivement en fonction du temps et envoi la valeurs des codeurs. +Réglage du gain des codeurs et asservissement des moteurs sur le retour des codeurs. + +Le code de démonstration fait avancer puis reculer, à vitesse constante, le robot. diff --git a/main.c b/main.c index 22abfb3..16d6ed9 100644 --- a/main.c +++ b/main.c @@ -4,6 +4,8 @@ * SPDX-License-Identifier: BSD-3-Clause */ #include "pico/stdlib.h" +#include "pico/multicore.h" +#include "Asser_Moteurs.h" #include "Moteurs.h" #include "QEI.h" #include "Teleplot.h" @@ -15,7 +17,11 @@ void setup(void); void loop(void); int valeur = 1; +int consigne_vitesse_moteur=200; uint32_t m_temps_ms; +uint32_t pas_de_temps_ms=1; + +void gestion_affichage(void); void main(void) { @@ -31,30 +37,37 @@ void setup(void){ Temps_init(); Moteur_init(); QEI_init(); + multicore_launch_core1(gestion_affichage); } void loop(void){ - static float cA_distance = 0, cB_distance = 0; - float t = (float) Temps_get_temps_ms() / 1000. ; - Moteur_set_commande(MOTEUR_A, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ; - Moteur_set_commande(MOTEUR_B, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ; - if(m_temps_ms != Temps_get_temps_ms()){ + if(Temps_get_temps_ms()!= m_temps_ms){ m_temps_ms = Temps_get_temps_ms(); - - if(m_temps_ms % 1 == 0){ // Fréquence d'actualisation des codeurs (1 ms) + if(m_temps_ms % pas_de_temps_ms == 0){ QEI_update(); - cA_distance += QEI_get_mm(QEI_A_NAME); - cB_distance += QEI_get_mm(QEI_B_NAME); + AsserMoteur_gestion(pas_de_temps_ms); + } + if(m_temps_ms % 1200 == 0){ + consigne_vitesse_moteur = -consigne_vitesse_moteur; + AsserMoteur_setConsigne_mm_s(MOTEUR_A, consigne_vitesse_moteur); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, consigne_vitesse_moteur); + } + } +} - if(m_temps_ms % 20 == 0){ // Fréquence d'affichage (20 ms) - Teleplot_fige_temps(); - Teleplot_add_variable_float_2decimal("codeur_A_vitesse", QEI_get_mm(QEI_A_NAME)); - Teleplot_add_variable_float_2decimal("codeur_B_vitesse", QEI_get_mm(QEI_B_NAME)); - Teleplot_add_variable_float_2decimal("codeur_A_distance", cA_distance); - Teleplot_add_variable_float_2decimal("codeur_B_distance", cA_distance); - Teleplot_envoie_tampon(); - } +void affichage(void){ + Teleplot_fige_temps(); + Teleplot_add_variable_int("consigne", consigne_vitesse_moteur); + // + Teleplot_add_variable_float_2decimal("Vitesse_A", QEI_get_mm(QEI_A_NAME) * 1000); + Teleplot_add_variable_float_2decimal("Vitesse_B", QEI_get_mm(QEI_B_NAME) * 1000); + Teleplot_envoie_tampon(); +} + +void gestion_affichage(void){ + while(1){ + affichage(); } } \ No newline at end of file