Asservissement des moteurs

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<MOTEUR_B+1; moteur++ ){
+        float erreur; // Erreur entre la consigne et la vitesse actuelle
+        float commande_P; // Terme proportionnel
+        float commande;
+        
+        // Calcul de l'erreur
+        erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
+
+        // Calcul du terme propotionnel
+        commande_P = erreur * ASSERMOTEUR_GAIN_P;
+
+        // Calcul du terme integral
+        commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
+
+        commande = commande_P + commande_I[moteur];
+
+        //Saturation de la commande
+        if(commande > 32760) {commande = 32760;}
+        if(commande < -32760) {commande = -32760;}
+
+        Moteur_set_commande(moteur, commande);
+
+    }
+}

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);

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_usb(PAMI_Cours_Asser_Moteurs 1)
-pico_enable_stdio_uart(PAMI_Cours_Codeurs 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
+    DEPENDS PAMI_Cours_Asser_Moteurs
-    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Codeurs.uf2
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/PAMI_Cours_Asser_Moteurs.uf2
 )

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;
 

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=400;
 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;
+	if(Temps_get_temps_ms()!= m_temps_ms){
-	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()){
 		m_temps_ms = Temps_get_temps_ms();
-
+		if(m_temps_ms % pas_de_temps_ms == 0){
-		if(m_temps_ms % 1 == 0){ // Fréquence d'actualisation des codeurs (1 ms)
 			QEI_update();
-			cA_distance += QEI_get_mm(QEI_A_NAME);
+			AsserMoteur_gestion(pas_de_temps_ms);
-			cB_distance += QEI_get_mm(QEI_B_NAME);
+			
+		}
+		if(m_temps_ms % 800 == 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)
+void affichage(void){
 	Teleplot_fige_temps();
-			Teleplot_add_variable_float_2decimal("codeur_A_vitesse", QEI_get_mm(QEI_A_NAME));
+	Teleplot_add_variable_int("consigne", consigne_vitesse_moteur);
-			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("Vitesse_A", QEI_get_mm(QEI_A_NAME) * 1000);
-			Teleplot_add_variable_float_2decimal("codeur_B_distance", cA_distance);
+	Teleplot_add_variable_float_2decimal("Vitesse_B", QEI_get_mm(QEI_B_NAME) * 1000);
 	Teleplot_envoie_tampon();
 }
+
+void gestion_affichage(void){
+	while(1){
+		affichage();
 	}
 
 }