From a3996a7db6ad102a83fc79a190570492f0ae5780 Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Fri, 18 Nov 2022 15:16:13 +0100
Subject: [PATCH] Test de compilation OK

---
 APDS_9960.c     |  2 +-
 Asser_Moteurs.c | 54 +++++++++++++++++++++++++++++++++++++++++++++++++
 Asser_Moteurs.h |  2 ++
 Moteurs.c       | 12 +++++------
 Moteurs.h       |  8 ++++----
 QEI.c           | 46 +++++++++++++++++++++++++++++------------
 QEI.h           |  7 ++++++-
 7 files changed, 106 insertions(+), 25 deletions(-)
 create mode 100644 Asser_Moteurs.c
 create mode 100644 Asser_Moteurs.h

diff --git a/APDS_9960.c b/APDS_9960.c
index 5abc159..7db55d2 100644
--- a/APDS_9960.c
+++ b/APDS_9960.c
@@ -1,4 +1,4 @@
-APDS9960_Init(){
+int APDS9960_Init(){
     // Registres à configurer 
 
 
diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c
new file mode 100644
index 0000000..5c1595f
--- /dev/null
+++ b/Asser_Moteurs.c
@@ -0,0 +1,54 @@
+#include "QEI.h"
+#include "Moteurs.h"
+
+/*** C'est ici que ce 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 (42.44f)
+#define ASSERMOTEUR_GAIN_P 20
+#define ASSERMOTEUR_GAIN_I 0.0f
+
+double consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
+double commande_I[3]; // Terme integral
+
+
+
+void AsserMoteur_setConsigne_mm_s(enum moteur_t moteur, double _consigne_mm_s){
+    consigne_mm_s[moteur] = _consigne_mm_s;
+
+}
+
+double AsserMoteur_getVitesse_mm_s(enum moteur_t moteur){
+    return (double) QEI_get(moteur) * (double)IMPULSION_PAR_MM;
+}
+
+AsserMoteur_Gestion(int step_ms){
+    // Pour chaque moteur
+    for(uint moteur=MOTEUR_A, i<MOTEUR_C+1; i++ ){
+        double erreur; // Erreur entre la consigne et la vitesse actuelle
+        double commande_P; // Terme proportionnel
+        double commande;
+        
+        // Calcul de l'erreur
+        erreur = consigne_mm_s - AsserMoteur_getVitesse_mm_s(moteur);
+
+        // 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_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..6cb89e4
--- /dev/null
+++ b/Asser_Moteurs.h
@@ -0,0 +1,2 @@
+void AsserMoteur_setConsigne_mm_s(enum moteur_t moteur, consigne_mm_s);
+AsserMoteur_Gestion(int step_ms);
\ No newline at end of file
diff --git a/Moteurs.c b/Moteurs.c
index 92305ca..58d53c0 100644
--- a/Moteurs.c
+++ b/Moteurs.c
@@ -44,21 +44,21 @@ void Moteur_Init(){
 
     pwm_config_set_clkdiv_int(&pwm_moteur, 2); 
     pwm_config_set_phase_correct(&pwm_moteur, false);
-    pwm_config_set_wrap(&pwm_moteur, 65635);
+    pwm_config_set_wrap(&pwm_moteur, (uint16_t)65635);
 
     pwm_init(slice_moteur_A, &pwm_moteur, true);
     pwm_init(slice_moteur_B, &pwm_moteur, true);
     pwm_init(slice_moteur_C, &pwm_moteur, true);
 
-    Moteur_set_vitesse(MOTEUR_A, 0);
-    Moteur_set_vitesse(MOTEUR_B, 0);
-    Moteur_set_vitesse(MOTEUR_C, 0);
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
 
 }
 
 
-Moteur_set_vitesse(enum t_moteur moteur, int16 vitesse){
-    uint16 u_vitesse;
+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){
diff --git a/Moteurs.h b/Moteurs.h
index c31983c..a44b080 100644
--- a/Moteurs.h
+++ b/Moteurs.h
@@ -1,10 +1,10 @@
 #include "pico/stdlib.h"
 
 enum t_moteur {
-    MOTEUR_A,
-    MOTEUR_B,
-    MOTEUR_C
+    MOTEUR_A=0,
+    MOTEUR_B=1,
+    MOTEUR_C=2
 };
 
 void Moteur_Init(void);
-void Moteur_SetVitesse(t_moteur moteur, int16 vitesse );
\ No newline at end of file
+void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse );
diff --git a/QEI.c b/QEI.c
index 3ed3fe1..233da1d 100644
--- a/QEI.c
+++ b/QEI.c
@@ -6,7 +6,7 @@
 #include "quadrature_encoder.pio.h"
 
 
-struct QEI_t QEI[3];
+struct QEI_t QEI_A, QEI_B, QEI_C;
 
 PIO pio_QEI = pio0;
 
@@ -31,25 +31,45 @@ void QEI_init(){
     // QEI3: broche 24 et 25 - pio : pio0, sm : 1, Offset : 0, broches 26 et 27, clock div : 0 pour commencer
     quadrature_encoder_program_init(pio_QEI, 2, offset, 24, 0);
 
-    QEI[0].value=0;
-    QEI[1].value=0;
-    QEI[2].value=0;
+    QEI_A.value=0;
+    QEI_B.value=0;
+    QEI_C.value=0;
 
 }
 
 void QEI_update(){
     int old_value;
 
-    old_value = QEI[0].value;
-    QEI[0].value = quadrature_encoder_get_count(pio_QEI, 0);
-    QEI[0].delta = QEI[0].value - old_value;
+    old_value = QEI_A.value;
+    QEI_A.value = quadrature_encoder_get_count(pio_QEI, 0);
+    QEI_A.delta = QEI_A.value - old_value;
 
-    old_value = QEI[1].value;
-    QEI[1].value = quadrature_encoder_get_count(pio_QEI, 1);
-    QEI[1].delta = QEI[1].value - old_value;
+    old_value = QEI_B.value;
+    QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1);
+    QEI_B.delta = QEI_B.value - old_value;
 
-    old_value = QEI[2].value;
-    QEI[2].value = quadrature_encoder_get_count(pio_QEI, 2);
-    QEI[2].delta = QEI[2].value - old_value;
+    old_value = QEI_C.value;
+    QEI_C.value = quadrature_encoder_get_count(pio_QEI, 2);
+    QEI_C.delta = QEI_C.value - old_value;
 
+}
+
+int QEI_get(enum t_moteur moteur){
+    switch (moteur)
+    {
+    case MOTEUR_A:
+        return QEI_A.value;
+        break;
+
+    case MOTEUR_B:
+        return QEI_B.value;
+        break;
+    
+    case MOTEUR_C:
+        return QEI_C.value;
+        break;
+    
+    default:
+        break;
+    }
 }
\ No newline at end of file
diff --git a/QEI.h b/QEI.h
index a761418..8814079 100644
--- a/QEI.h
+++ b/QEI.h
@@ -1,7 +1,12 @@
+#include "Moteurs.h"
+
 struct QEI_t{
     int value;
     int delta;
 };
 
+extern struct QEI_t QEI_A, QEI_B, QEI_C;
+
 void QEI_update(void);
-void QEI_init(void);
\ No newline at end of file
+void QEI_init(void);
+int QEI_get(enum t_moteur moteur);
\ No newline at end of file