Asservissement moteurs - fonctionnel ?

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

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
 )
 

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

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

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

Temps.c

@@ -0,0 +1,24 @@
+#include <stdio.h>
+#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;
+}

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

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 <stdio.h>
 #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();
 		
-		printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );	
+		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) );	
+			}
+		}
 
-		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);
-	}
-}