From 15c9f9b6c5d2b3c6f96063c37430a7eb37e9f1ef Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 15 Jan 2025 11:06:48 +0100 Subject: [PATCH] Code de demonstration du pilotage des moteurs --- CMakeLists.txt | 21 ++++++------ Moteurs.c | 86 ++++++++++++++++++++++++++++++++++++++++++++++++++ Moteurs.h | 15 +++++++++ Readme.md | 9 +++--- main.c | 34 ++++---------------- 5 files changed, 124 insertions(+), 41 deletions(-) create mode 100644 Moteurs.c create mode 100644 Moteurs.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 165445d..8aceef8 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_temps C CXX ASM) +project(PAMI_Cours_Moteurs C CXX ASM) set(CMAKE_C_STNDARD 11) set(CMAKE_CXX_STANDARD 17) @@ -10,27 +10,30 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR}) pico_sdk_init() -add_executable(PAMI_Cours_temps +add_executable(PAMI_Cours_Moteurs main.c + + Moteurs.c Teleplot.c Temps.c ) -target_include_directories(PAMI_Cours_temps PRIVATE ${CMAKE_CURRENT_LIST_DIR}) +target_include_directories(PAMI_Cours_Moteurs PRIVATE ${CMAKE_CURRENT_LIST_DIR}) -target_link_libraries(PAMI_Cours_temps +target_link_libraries(PAMI_Cours_Moteurs hardware_uart + hardware_pwm pico_stdlib pico_multicore pico_cyw43_arch_lwip_poll ) -pico_enable_stdio_usb(PAMI_Cours_temps 1) -pico_enable_stdio_uart(PAMI_Cours_temps 1) +pico_enable_stdio_usb(PAMI_Cours_Moteurs 1) +pico_enable_stdio_uart(PAMI_Cours_Moteurs 1) -pico_add_extra_outputs(PAMI_Cours_temps) +pico_add_extra_outputs(PAMI_Cours_Moteurs) add_custom_target(Flash - DEPENDS PAMI_Cours_temps - COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_temps.uf2 + DEPENDS PAMI_Cours_Moteurs + COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Moteurs.uf2 ) diff --git a/Moteurs.c b/Moteurs.c new file mode 100644 index 0000000..cd4b9d5 --- /dev/null +++ b/Moteurs.c @@ -0,0 +1,86 @@ +#include "hardware/pwm.h" +#include "Moteurs.h" + +#define MOTEUR_A_SENS1 7 +#define MOTEUR_A_SENS2 13 +#define MOTEUR_A_VITESSE 27 //5B +#define MOTEUR_B_SENS1 10 +#define MOTEUR_B_SENS2 5 +#define MOTEUR_B_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(MOTEUR_A_SENS1); + gpio_init(MOTEUR_A_SENS2); + gpio_init(MOTEUR_B_SENS1); + gpio_init(MOTEUR_B_SENS2); + gpio_set_dir(MOTEUR_A_SENS1, GPIO_OUT); + gpio_set_dir(MOTEUR_A_SENS2, GPIO_OUT); + gpio_set_dir(MOTEUR_B_SENS1, GPIO_OUT); + gpio_set_dir(MOTEUR_B_SENS2, GPIO_OUT); + gpio_put(MOTEUR_A_SENS1, 0); + gpio_put(MOTEUR_A_SENS2, 0); + gpio_put(MOTEUR_B_SENS1, 0); + gpio_put(MOTEUR_B_SENS2, 0); + + gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM); + gpio_set_function(MOTEUR_B_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_set_commande(MOTEUR_A, 0); + Moteur_set_commande(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_set_commande(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(MOTEUR_A_SENS1, 0); + gpio_put(MOTEUR_A_SENS2, 1); + }else{ + gpio_put(MOTEUR_A_SENS1, 1); + gpio_put(MOTEUR_A_SENS2, 0); + } + break; + + case MOTEUR_B: + pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse); + if(vitesse < 0){ + gpio_put(MOTEUR_B_SENS1, 0); + gpio_put(MOTEUR_B_SENS2, 1); + }else{ + gpio_put(MOTEUR_B_SENS1, 1); + gpio_put(MOTEUR_B_SENS2, 0); + } + break; + + } + +} + +void Moteur_stop(void){ + Moteur_set_commande(MOTEUR_A, 0); + Moteur_set_commande(MOTEUR_B, 0); +} \ No newline at end of file diff --git a/Moteurs.h b/Moteurs.h new file mode 100644 index 0000000..70aaf99 --- /dev/null +++ b/Moteurs.h @@ -0,0 +1,15 @@ +#include "pico/stdlib.h" + +#ifndef MOTEURS_H +#define MOTEURS_H +enum t_moteur { + MOTEUR_A=0, + MOTEUR_B=1 +}; +#endif + +#define MOTEUR_MAX_COMMANDE 32767 + +void Moteur_init(void); +void Moteur_set_commande(enum t_moteur moteur, int16_t vitesse ); +void Moteur_stop(void); diff --git a/Readme.md b/Readme.md index 91b1a40..c66331e 100644 --- a/Readme.md +++ b/Readme.md @@ -1,9 +1,10 @@ -De l’art de déplacer un robot avec classe - Câblage des moteurs -=============================================================== +De l’art de déplacer un robot avec classe - Commande 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) -Câblage des moteurs +Commande des moteurs ------------------- -Le code principal permet de tester le bon fonctionnement des moteurs, afin de valider leur câblage. +Ajout des fichiers pour gérer les moteurs. Le code principal fait évoluer la vitesse des moteurs progressivement en fonction du temps. + diff --git a/main.c b/main.c index b5adaa1..a8ca695 100644 --- a/main.c +++ b/main.c @@ -4,9 +4,11 @@ * SPDX-License-Identifier: BSD-3-Clause */ #include "pico/stdlib.h" +#include "Moteurs.h" #include "Teleplot.h" #include "Temps.h" #include +#include void setup(void); void loop(void); @@ -25,35 +27,11 @@ void setup(void){ stdio_init_all(); Teleplot_init(); Temps_init(); - - // Broches des moteurs gérées en GPIO - gpio_init(7); - gpio_init(13); - gpio_init(27); - gpio_init(5); - gpio_init(10); - gpio_init(9); - - // Broches des moteurs en sortie - gpio_set_dir(7, true); // M1 Sens 1 - gpio_set_dir(13, true); // M1 Sens 2 - gpio_set_dir(27, true); // M1 Vitesse - - gpio_set_dir(5, true); // M2 Sens 2 - gpio_set_dir(10, true); // M2 Sens 1 - gpio_set_dir(9, true); // M2 Vitesse - - // Commande des broches - gpio_put(7, 0); - gpio_put(13, 1); - gpio_put(27, 1); - - gpio_put(5, 0); - gpio_put(10, 1); - gpio_put(9, 1); - + Moteur_init(); } void loop(void){ - tight_loop_contents(); + float t = (float) Temps_get_temps_ms() / 1000. ; + Moteur_set_commande(MOTEUR_A, sin(t * 3.14) * MOTEUR_MAX_COMMANDE) ; + Moteur_set_commande(MOTEUR_B, sin(t * 3.14) * MOTEUR_MAX_COMMANDE) ; } \ No newline at end of file