From c02b3ef667e5b8355b55b3cb5900d689232cd5bc Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Thu, 17 Nov 2022 18:43:11 +0100
Subject: [PATCH] =?UTF-8?q?Code=20de=20contr=C3=B4le=20basic=20des=20moteu?=
 =?UTF-8?q?rs=20-=20non=20test=C3=A9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 CMakeLists.txt |  2 ++
 Moteurs.c      | 98 ++++++++++++++++++++++++++++++++++++++++++++++++++
 Moteurs.h      | 10 ++++++
 3 files changed, 110 insertions(+)
 create mode 100644 Moteurs.c
 create mode 100644 Moteurs.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f2c8871..a599d86 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,6 +16,8 @@ Servomoteur.c
 gyro_L3GD20H.c
 gyro_ADXRS453.c
 QEI.c
+APDS_9960.c
+Moteurs.c
 )
 
 pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
diff --git a/Moteurs.c b/Moteurs.c
new file mode 100644
index 0000000..92305ca
--- /dev/null
+++ b/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
+
+uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
+
+
+void Moteur_Init(){
+    // Initialisation des broches pour les PWM
+    gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM);
+    gpio_set_function(MOTEUR_B_VITESSE, GPIO_FUNC_PWM);
+    gpio_set_function(MOTEUR_C_VITESSE, GPIO_FUNC_PWM);
+
+    // Initialisation des broches pour les sens
+    gpio_init(MOTEUR_A_SENS);
+    gpio_init(MOTEUR_B_SENS);
+    gpio_init(MOTEUR_C_SENS);
+    gpio_set_dir(MOTEUR_A_SENS, true);
+    gpio_set_dir(MOTEUR_B_SENS, true);
+    gpio_set_dir(MOTEUR_C_SENS, true);
+
+    // Configuration des PWM à 1 kHz
+    // Clock_system 125 MHz - 16 bits PWM (65536)
+    // Donc si on veut profiter de toute la résolution du PWM,
+    // il faut une fréquence d'entrée à 65,536 MHz
+    // Soit un diviseur de 2 (apprximativement)
+
+    slice_moteur_A = pwm_gpio_to_slice_num(MOTEUR_A_VITESSE); // GPIO6  -> PWM 3A (voir RP2040 Datasheet 4.5.2)
+    slice_moteur_B = pwm_gpio_to_slice_num(MOTEUR_B_VITESSE); // GPIO8  -> PWM 4A
+    slice_moteur_C = pwm_gpio_to_slice_num(MOTEUR_C_VITESSE); // GPIO10 -> PWM 5A
+
+    pwm_config pwm_moteur = pwm_get_default_config();
+
+    pwm_config_set_clkdiv_int(&pwm_moteur, 2); 
+    pwm_config_set_phase_correct(&pwm_moteur, false);
+    pwm_config_set_wrap(&pwm_moteur, 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_set_vitesse(enum t_moteur moteur, int16 vitesse){
+    uint16 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;
+    }
+    u_vitesse = vitesse * 2;
+
+    switch(moteur){
+        case MOTEUR_A:
+            pwm_set_chan_level(slice_moteur_A, PWM_CHAN_A, u_vitesse);
+            if(vitesse < 0){
+                gpio_put(MOTEUR_A_SENS, false);
+            }else{
+                gpio_put(MOTEUR_A_SENS, true);
+            }
+            break;
+
+        case MOTEUR_B:
+            pwm_set_chan_level(slice_moteur_B, PWM_CHAN_A, u_vitesse);
+            if(vitesse < 0){
+                gpio_put(MOTEUR_B_SENS, false);
+            }else{
+                gpio_put(MOTEUR_B_SENS, true);
+            }
+            break;
+
+        case MOTEUR_C:
+            pwm_set_chan_level(slice_moteur_C, PWM_CHAN_A, u_vitesse);
+            if(vitesse < 0){
+                gpio_put(MOTEUR_B_SENS, false);
+            }else{
+                gpio_put(MOTEUR_B_SENS, true);
+            }
+            break;
+    }
+
+}
\ No newline at end of file
diff --git a/Moteurs.h b/Moteurs.h
new file mode 100644
index 0000000..c31983c
--- /dev/null
+++ b/Moteurs.h
@@ -0,0 +1,10 @@
+#include "pico/stdlib.h"
+
+enum t_moteur {
+    MOTEUR_A,
+    MOTEUR_B,
+    MOTEUR_C
+};
+
+void Moteur_Init(void);
+void Moteur_SetVitesse(t_moteur moteur, int16 vitesse );
\ No newline at end of file