From a170a48e85a0b6a49ec5d98c6a22cabe4f2c9b7f Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Wed, 30 Nov 2022 21:14:47 +0100
Subject: [PATCH] =?UTF-8?q?Gestion=20des=20unit=C3=A9s=20dans=20QEI=20dire?=
 =?UTF-8?q?ctement?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 QEI.c | 22 ++++++++++++++++++++++
 QEI.h |  3 ++-
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/QEI.c b/QEI.c
index 9647b5e..76b6907 100644
--- a/QEI.c
+++ b/QEI.c
@@ -6,6 +6,21 @@
 #include "quadrature_encoder.pio.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 (42.44f)
+#define ASSERMOTEUR_GAIN_P 160
+#define ASSERMOTEUR_GAIN_I .80f
+
 struct QEI_t QEI_A, QEI_B, QEI_C;
 
 PIO pio_QEI = pio0;
@@ -89,4 +104,11 @@ int QEI_get(enum QEI_name_t qei){
     default:
         break;
     }
+}
+
+/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente
+/// @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()
+double QEI_get_mm(enum QEI_name_t qei){
+    return (double) QEI_get(qei) / (double)IMPULSION_PAR_MM;
 }
\ No newline at end of file
diff --git a/QEI.h b/QEI.h
index d800e55..607a09c 100644
--- a/QEI.h
+++ b/QEI.h
@@ -13,4 +13,5 @@ extern struct QEI_t QEI_A, QEI_B, QEI_C;
 
 void QEI_update(void);
 void QEI_init(void);
-int QEI_get(enum QEI_name_t qei);
\ No newline at end of file
+int QEI_get(enum QEI_name_t qei);
+double QEI_get_mm(enum QEI_name_t qei);
\ No newline at end of file