Correction des positions de démarrage

Lecture des consignes de vitesse pour commander les moteurs

.vscode/settings.json

@@ -1,5 +1,9 @@
 {
     "files.associations": {
-
+        "stdlib.h": "c",
+        "stdio.h": "c",
+        "i2c_slave.h": "c",
+        "i2c.h": "c",
+        "moteur.h": "c"
     }
 }

CMakeLists.txt

@@ -12,12 +12,19 @@ pico_sdk_init()
 
 add_executable(Mon_Projet
   main.c
+  i2c_maitre.c
+  i2c_slave.c
+  communication.c
+  moteur.c
+  Servomoteur.c
 )
 
 target_include_directories(Mon_Projet PRIVATE Mon_Projet_ULD_API/inc/)
 
 target_link_libraries(Mon_Projet
   hardware_i2c 
+  hardware_adc 
+  hardware_pwm
   hardware_uart 
   pico_stdlib
   pico_multicore

Servomoteur.c

@@ -0,0 +1,88 @@
+#include "moteur.h"
+
+// Define pins servo
+#define SERVO1_PIN 20
+#define SERVO2_PIN 21
+
+// S0 : 26 - PWM 5 - Chan A
+// S1 : 22 - PWM 3 - Chan A
+// S2 : 21 - PWM 2 - Chan B
+// S3 : 20 - PWM 2 - Chan A
+// S4 : 27 - PWM 5 - Chan B
+
+void Servomoteur_init(){
+    // Set wrap of pwm slice
+	pwm_set_wrap(2, 25000);
+    pwm_set_wrap(3, 25000);
+    pwm_set_wrap(5, 25000);
+
+	// Set clock div for this pwm slice
+	pwm_set_clkdiv(2, 100);
+    pwm_set_clkdiv(3, 100);
+    pwm_set_clkdiv(5, 100);
+
+	// Enable pwm slice
+	pwm_set_enabled(2, true);
+    pwm_set_enabled(3, true);
+    pwm_set_enabled(5, true);
+
+	// Set channel level
+	pwm_set_chan_level(2, PWM_CHAN_A, 2800); // S3 - Ouvert : 1700 - Fermé : 2800
+	pwm_set_chan_level(2, PWM_CHAN_B, 2650); // S2 - Ouvert : 1500 - Fermé : 2650
+    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
+	pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - 1750
+    pwm_set_chan_level(5, PWM_CHAN_B, 1500); // S4 - Ouvert 2700 - Fermé : 1500 
+
+
+    // Init pin for the servos
+	gpio_init(SERVO1_PIN);
+	gpio_init(SERVO2_PIN);
+    gpio_init(22);
+    gpio_init(26);
+	gpio_init(27);
+	gpio_set_function(SERVO1_PIN, GPIO_FUNC_PWM);
+	gpio_set_function(SERVO2_PIN, GPIO_FUNC_PWM);
+	gpio_set_function(22, GPIO_FUNC_PWM);
+	gpio_set_function(26, GPIO_FUNC_PWM);
+	gpio_set_function(27, GPIO_FUNC_PWM);
+
+	// Edit 17/01/2024
+	// Réglage servomoteurs en 25000 * 100
+	// Pince
+	// Pot : 1700
+	// Plante : 1970
+	// Ouvert : 1000
+	// Ascenseur
+	// Haut : 2550
+	// Bas : 1550
+	// Lâche plante : 2050
+	// Lâche pot jardinière : 1950
+}
+
+void Servo_pince_tient(){
+    pwm_set_chan_level(2, PWM_CHAN_A, 2800);
+    pwm_set_chan_level(5, PWM_CHAN_B, 1350);
+    pwm_set_chan_level(2, PWM_CHAN_B, 2650);
+}
+
+void Servo_pince_lache(){
+    pwm_set_chan_level(2, PWM_CHAN_A, 1700);
+    pwm_set_chan_level(5, PWM_CHAN_B, 2550);
+    pwm_set_chan_level(2, PWM_CHAN_B, 1500);
+}
+
+void Servo_came_pousse(){
+    pwm_set_chan_level(3, PWM_CHAN_A, 2300); // S1 - 2300 Pousse, 1500 Neutre
+}
+
+void Servo_came_neutre(){
+    pwm_set_chan_level(3, PWM_CHAN_A, 1500); // S1 - 2300 Pousse, 1500 Neutre
+}
+
+void Servo_deplie_banderole(){
+    pwm_set_chan_level(5, PWM_CHAN_A, 900); // S0 - Dépose 900 - neutre 1750
+}
+
+void Servo_plie_banderole(){
+    pwm_set_chan_level(5, PWM_CHAN_A, 1750); // S0 - Dépose 900 - neutre 1750
+}

Servomoteur.h

@@ -0,0 +1,9 @@
+void Servomoteur_init();
+void Servo_pince_tient();
+void Servo_pince_lache();
+
+void Servo_came_pousse();
+void Servo_came_neutre();
+
+void Servo_deplie_banderole();
+void Servo_plie_banderole();

communication.c

@@ -0,0 +1,107 @@
+/*****
+ * 
+ * Le principe est que la télécommande soit l'esclave I2C
+ * Pour envoyer un message, on charge le message à l'adresse 0 de la "mémoire" pour l'i2c
+ * 
+ * Pour lire le message, le robot interroge la télécommande, et demande le contenu à partir de l'adresse 0 de la mémoire.
+ * Ainsi, en cas d'échec de la communication, le robot détectera une manette débranchée.
+ * Par défaut, la fonction lit 255 caractères.
+ * 
+ * Copyright (c) 2024 - Club robotique de Riom
+ * 
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+*/
+#include "i2c_fifo.h"
+#include "i2c_slave.h"
+#include "i2c_maitre.h"
+#include "string.h"
+
+
+/// DEBUT DE LA CONFIGURATION de L'I2C
+
+#define I2C0_SDA_PIN 16
+#define I2C0_SCL_PIN 17
+
+#define I2C_SLAVE_ADDRESS 0x17
+
+static const uint I2C_SLAVE_SDA_PIN = I2C0_SDA_PIN; 
+static const uint I2C_SLAVE_SCL_PIN = I2C0_SCL_PIN;
+
+// The slave implements a 256 byte memory. To write a series of bytes, the master first
+// writes the memory address, followed by the data. The address is automatically incremented
+// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
+// sequentially from the current memory address.
+static struct
+{
+    uint8_t mem[256];
+    uint8_t mem_address;
+    bool mem_address_written;
+} context;
+
+// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
+// printing to stdio may interfere with interrupt handling.
+static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+    switch (event) {
+    case I2C_SLAVE_RECEIVE: // master has written some data
+        if (!context.mem_address_written) {
+            // writes always start with the memory address
+            context.mem_address = i2c_read_byte(i2c);
+            context.mem_address_written = true;
+        } else {
+            // save into memory
+            context.mem[context.mem_address] = i2c_read_byte(i2c);
+            context.mem_address++;
+        }
+        break;
+    case I2C_SLAVE_REQUEST: // master is requesting data
+        // load from memory
+        i2c_write_byte(i2c, context.mem[context.mem_address]);
+        context.mem_address++;
+        break;
+    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
+        context.mem_address_written = false;
+        break;
+    default:
+        break;
+    }
+}
+
+void i2c_set_slave_mode_perso(i2c_inst_t *i2c, uint8_t addr) {
+    i2c->hw->enable = 0;
+    
+    //while( !(i2c->hw->enable_status & 0x1) );
+    
+    i2c->hw->sar = addr;
+    i2c->hw->con = 0;
+    
+    i2c->hw->enable = 1;
+}
+
+static void setup_slave() {
+    gpio_init(I2C_SLAVE_SDA_PIN);
+    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SLAVE_SDA_PIN);
+
+    gpio_init(I2C_SLAVE_SCL_PIN);
+    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SLAVE_SCL_PIN);
+
+    i2c_slave_init(i2c0, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
+}
+/// FIN DE LA CONFIGURATION de L'I2C
+
+
+void communication_init(void){
+    setup_slave();
+    i2c_maitre_init();
+}
+
+void communication_send_message(unsigned char * message, unsigned int message_length){
+    memcpy(context.mem, message, message_length);
+}
+
+int communication_read_message(unsigned char * message){
+    i2c_lire_registre(I2C_SLAVE_ADDRESS, 0, message, 25);
+
+}

communication.h

@@ -0,0 +1,5 @@
+#include "i2c_maitre.h"
+
+void communication_init(void);
+void communication_send_message(unsigned char * message, unsigned int message_length);
+enum i2c_resultat_t communication_read_message(unsigned char * message);

i2c_fifo.h

@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef _I2C_FIFO_H_
+#define _I2C_FIFO_H_
+
+#include <hardware/i2c.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file i2c_fifo.h
+ *
+ * \brief I2C non-blocking r/w.
+ */
+
+/**
+ * \brief Pop a byte from I2C Rx FIFO.
+ * 
+ * This function is non-blocking and assumes the Rx FIFO isn't empty.
+ * 
+ * \param i2c I2C instance.
+ * \return uint8_t Byte value.
+ */
+static inline uint8_t i2c_read_byte(i2c_inst_t *i2c) {
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    assert(hw->status & I2C_IC_STATUS_RFNE_BITS); // Rx FIFO must not be empty
+    return (uint8_t)hw->data_cmd;
+}
+
+/**
+ * \brief Push a byte into I2C Tx FIFO.
+ * 
+ * This function is non-blocking and assumes the Tx FIFO isn't full.
+ * 
+ * \param i2c I2C instance.
+ * \param value Byte value.
+ */
+static inline void i2c_write_byte(i2c_inst_t *i2c, uint8_t value) {
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    assert(hw->status & I2C_IC_STATUS_TFNF_BITS); // Tx FIFO must not be full
+    hw->data_cmd = value;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _I2C_FIFO_H_

i2c_maitre.c

@@ -0,0 +1,281 @@
+#include "i2c_maitre.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "pico/stdlib.h"
+#include <stdio.h>
+
+#define I2C1_SDA_PIN 18
+#define I2C1_SCL_PIN 19
+
+#define I2C_NB_MAX_TAMPON 20
+
+enum i2c_statu_t{
+    I2C_STATU_LIBRE,
+    I2C_STATU_OCCUPE
+} i2c_statu_i2c1;
+
+uint16_t I2C_tampon_envoi[I2C_NB_MAX_TAMPON];
+uint8_t I2C_tampon_reception[I2C_NB_MAX_TAMPON];
+uint16_t I2C_nb_a_envoyer, I2C_nb_a_recevoir;
+uint8_t adresse_7_bits;
+uint32_t i2c_error_code; // value of i2c->hw->tx_abrt_source if anything wrong happen, 0 if everything was fine.
+
+enum transaction_statu_t{
+    TRANSACTION_EN_COURS,
+    TRANSACTION_TERMINEE
+} statu_emission, statu_reception;
+
+void i2d_set_adresse_esclave(uint8_t _adresse_7bits);
+void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
+enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception);
+
+void i2c_maitre_init(void){
+    //stdio_init_all();
+    i2c_init(i2c1, 10 * 1000);
+
+    printf("Initialisation des broches\n");
+    for(int i=0; i++; i<=28){
+        if(gpio_get_function(i) == GPIO_FUNC_I2C){
+            printf("Borche I2C : %d\n", i);
+            gpio_set_function(i, GPIO_FUNC_NULL);
+        }
+    }
+
+    printf("%d et %d en I2C\n", I2C1_SDA_PIN, I2C1_SCL_PIN);
+    gpio_set_function(I2C1_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C1_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C1_SDA_PIN);
+    gpio_pull_up(I2C1_SCL_PIN);
+
+    i2c_statu_i2c1 = I2C_STATU_LIBRE;
+}
+
+/// @brief Fonction à appeler régulièrement ou en interruption.
+/// @param i2c 
+void i2c_gestion(i2c_inst_t *i2c){
+    // on veut gérer l'i2c avec cette fonction.
+    // 2 cas :
+    // - Soit écriture simple (plusieurs octets (W))
+    // - Soit écriture + lecture (Adresse (W), registre (W), données (R))
+    // Pour écrire 1 octet, i2c->hw->data_cmd = xxx, (avec CMD:8 à 0, ) 
+    // Pour lire 1 octet, i2c->hw->data_cmd = xxx (avec CMD:8 à 1)
+    // Il faut mettre CMD:9 à 1 pour le dernier octet.
+
+    // Envoi des données (ou des demandes de lecture)
+    static uint16_t index_envoi=0, index_reception=0;
+
+    // Acquitement des erreurs, pas 100% fonctionnel ! TODO !
+    if(i2c->hw->tx_abrt_source !=0){
+        // Seule solution trouvée pour réinitialiser l'I2C.
+        char a;
+        i2c_read_blocking(i2c, adresse_7_bits, &a, 1, false);
+
+        I2C_nb_a_envoyer = 0;
+        index_reception = 0;
+        I2C_nb_a_recevoir = 0;
+        statu_emission = TRANSACTION_TERMINEE;
+        statu_reception = TRANSACTION_TERMINEE;
+        i2c_statu_i2c1 = I2C_STATU_LIBRE;
+    }
+
+    while( (index_envoi < I2C_nb_a_envoyer) && (i2c_get_write_available(i2c)) ){
+        bool restart = false;
+        bool last = false;
+
+        if (index_envoi == 0){
+            // Début de l'envoi, assurons nous d'avoir la bonne adresse de l'esclave
+            i2c->hw->enable = 0;
+            i2c->hw->tar = adresse_7_bits;
+            i2c->hw->enable = 1;
+        }else{
+            // Passage de l'écriture à la lecture, on envoie un bit de restart.
+            if( !(I2C_tampon_envoi[index_envoi-1] & I2C_IC_DATA_CMD_CMD_BITS) && 
+                    (I2C_tampon_envoi[index_envoi] & I2C_IC_DATA_CMD_CMD_BITS)){
+                restart = true;
+            }
+        }
+
+        if(index_envoi + 1 == I2C_nb_a_envoyer){
+            // Fin de la trame, nous devons envoyer un bit de stop.
+            last = true;
+        }
+
+        i2c->hw->data_cmd =
+                I2C_tampon_envoi[index_envoi] |
+                bool_to_bit(restart) << I2C_IC_DATA_CMD_RESTART_LSB |
+                bool_to_bit(last) << I2C_IC_DATA_CMD_STOP_LSB;
+
+        if(last){
+            statu_emission = TRANSACTION_TERMINEE;
+            index_envoi = 0;
+            I2C_nb_a_envoyer = 0;
+            //printf("I2C emission terminee\n");
+        }else{
+            index_envoi++;
+        }
+        
+    }
+
+    // Réception des données - Lecture des données présentes dans le tampon
+    while( (index_reception < I2C_nb_a_recevoir) && (i2c_get_read_available(i2c)) ){
+        I2C_tampon_reception[index_reception] = (uint8_t) i2c->hw->data_cmd;
+        index_reception++;
+    }
+    if(index_reception == I2C_nb_a_recevoir){
+        statu_reception = TRANSACTION_TERMINEE;
+        index_reception = 0;
+        I2C_nb_a_recevoir = 0;
+    }
+
+    if(statu_reception == TRANSACTION_TERMINEE && statu_emission == TRANSACTION_TERMINEE){
+        i2c_statu_i2c1 = I2C_STATU_LIBRE;
+    }
+
+}
+
+/// @brief Charge le tampon d'émission pour pré-mâcher le travail à la fonction i2c_gestion
+/// @param emission 
+/// @param nb_envoi 
+/// @param nb_reception 
+void i2c_charger_tampon_envoi(uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
+    // Données à envoyer
+    for(unsigned int index=0; index<nb_envoi; index++){
+        I2C_tampon_envoi[index] = (uint16_t) emission[index];
+    }
+    // Données à lire
+    for(unsigned int index=0; index<nb_reception; index++){
+        I2C_tampon_envoi[nb_envoi + index] = (uint16_t) 0x0100;
+    }
+}
+
+/// @brief Stock l'adresse de l'esclave avec lequel communiquer
+/// @param _adresse_7bits 
+void i2d_set_adresse_esclave(uint8_t _adresse_7bits){
+    adresse_7_bits =_adresse_7bits;
+}
+
+/// @brief Initialise la transmission I2, sur l'i2c1. Une transmission se compose de 2 trames I2C, une pour écrire (Adresse + données), une pour lire
+/// Si nb_reception = 0, alors la trame pour lire ne sera pas envoyée.
+/// @param emission : données à envoyer
+/// @param nb_envoi : nombre de données à envoyer
+/// @param nb_reception : nombre de données à recevoir
+/// @return I2C_EN_COURS, I2C_SUCCES ou I2C_ECHEC
+enum i2c_resultat_t i2c_transmission(uint8_t _adresse_7bits, uint8_t* emission, uint16_t nb_envoi, uint16_t nb_reception){
+    static enum m_statu_t{
+        I2C_STATU_INIT,
+        I2C_STATU_EN_COURS,
+    }m_statu = I2C_STATU_INIT;
+
+    switch(m_statu){
+        case I2C_STATU_INIT:
+            // I2C libre ?
+            if(i2c_statu_i2c1 == I2C_STATU_OCCUPE){
+                return I2C_EN_COURS;
+            }
+            // Alors il est à nous !
+            i2c_statu_i2c1 = I2C_STATU_OCCUPE;
+            statu_emission = TRANSACTION_EN_COURS;
+            statu_reception = TRANSACTION_EN_COURS;
+            i2c_error_code = 0;
+
+            i2d_set_adresse_esclave(_adresse_7bits);
+
+            i2c_charger_tampon_envoi(emission, nb_envoi, nb_reception);
+            // Nous devons envoyer aussi une commande pour chaque octet à recevoir.
+            I2C_nb_a_envoyer = nb_envoi + nb_reception;
+            I2C_nb_a_recevoir = nb_reception;
+            
+            // On appelle la fonction gestion pour gagner du temps.
+            i2c_gestion(i2c1);
+            m_statu = I2C_STATU_EN_COURS;
+            break;
+
+        case I2C_STATU_EN_COURS:
+            if(i2c_statu_i2c1 == I2C_STATU_LIBRE){
+                m_statu = I2C_STATU_INIT;
+                if(i2c_error_code){
+                    return I2C_ECHEC;
+                }else{
+                    return I2C_SUCCES;
+                }
+                
+            }
+            break;
+    }
+    return I2C_EN_COURS;
+}
+
+/// @brief Lit le registre d'un composant se comportant comme une EPROM I2C.
+/// @return I2C_SUCCES, I2C_EN_COURS ou I2C_ECHEC
+enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len){
+    uint8_t emission[1];
+    emission[0] = registre;
+    enum i2c_resultat_t i2c_resultat;
+    i2c_resultat = i2c_transmission(adresse_7_bits, emission, 1, len);
+    if(i2c_resultat == I2C_SUCCES){
+        for(uint32_t i = 0; i < len; i++){
+            reception[i] = I2C_tampon_reception[i];
+        }
+        return I2C_SUCCES;
+    }else if(i2c_resultat == I2C_ECHEC){
+        return I2C_ECHEC;
+    }
+    return I2C_EN_COURS;   
+}
+
+/// @brief Initialise une transaction I2C. 
+/// Renvoie I2C_SUCCES si l'intégralité du message est chargé en envoi, 
+/// Renvoie I2C_EN_COURS si la fonction doit encore être appelée pour finir d'envoyer le message
+/// Renvoie I2C_ECHEC en cas d'erreur I2C.
+enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len){
+    uint8_t emission[I2C_NB_MAX_TAMPON];
+    emission[0] = registre;
+    for(uint32_t i = 0; i < len; i++){
+        emission[i+1] = _emission[i];
+    }
+    enum i2c_resultat_t i2c_resultat;
+    return i2c_transmission(adresse_7_bits, emission, 1 + len, 0);
+    
+}
+
+
+/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt => Non, voir i2c_lire_registre_nb
+/// @param adresse_7_bits 
+/// @param  
+/// @return I2C_SUCCES (1) ou I2C_ECHEC (2)
+int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len){
+    int statu;
+    char emission[1];
+
+    emission[0] = registre;
+    statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 1, 0);
+    if(statu == PICO_ERROR_GENERIC){
+        printf("I2C - Envoi registre Echec\n");
+        return I2C_ECHEC;
+    }
+
+    statu = i2c_read_blocking (i2c1, adresse_7_bits, reception, len, 0);
+    if(statu == PICO_ERROR_GENERIC){
+        printf("I2C - Lecture registre Echec\n");
+        return I2C_ECHEC;
+    }
+
+    return I2C_SUCCES;
+}
+
+int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){
+    int statu;
+    char emission[2];
+
+    emission[0] = registre;
+    emission[1] = valeur_registre;
+    statu = i2c_write_blocking (i2c1, adresse_7_bits, emission, 2, 0);
+    if(statu == PICO_ERROR_GENERIC){
+        printf("Erreur ecrire registre\n");
+        return I2C_ECHEC;
+    }
+
+    printf("i2c Registre %x, valeur %x\n", registre, valeur_registre);
+
+    return I2C_SUCCES;
+}

i2c_maitre.h

@@ -0,0 +1,15 @@
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+
+enum i2c_resultat_t {
+    I2C_EN_COURS,
+    I2C_SUCCES,
+    I2C_ECHEC
+};
+
+void i2c_maitre_init(void);
+void i2c_gestion(i2c_inst_t *i2c);
+enum i2c_resultat_t i2c_lire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * reception, uint8_t len);
+enum i2c_resultat_t i2c_ecrire_registre_nb(uint8_t adresse_7_bits, uint8_t registre, uint8_t * _emission, uint8_t len);
+int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre);
+int i2c_lire_registre(char adresse_7_bits, char registre, unsigned char * reception, char len);

i2c_slave.c

@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include "i2c_slave.h"
+#include "hardware/irq.h"
+
+typedef struct i2c_slave_t
+{
+    i2c_inst_t *i2c;
+    i2c_slave_handler_t handler;
+    bool transfer_in_progress;
+} i2c_slave_t;
+
+static i2c_slave_t i2c_slaves[2];
+
+static inline void finish_transfer(i2c_slave_t *slave) {
+    if (slave->transfer_in_progress) {
+        slave->handler(slave->i2c, I2C_SLAVE_FINISH);
+        slave->transfer_in_progress = false;
+    }
+}
+
+static void __not_in_flash_func(i2c_slave_irq_handler)(i2c_slave_t *slave) {
+    i2c_inst_t *i2c = slave->i2c;
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+
+    uint32_t intr_stat = hw->intr_stat;
+    if (intr_stat == 0) {
+        return;
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
+        hw->clr_tx_abrt;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
+        hw->clr_start_det;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
+        hw->clr_stop_det;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
+        slave->transfer_in_progress = true;
+        slave->handler(i2c, I2C_SLAVE_RECEIVE);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
+        hw->clr_rd_req;
+        slave->transfer_in_progress = true;
+        slave->handler(i2c, I2C_SLAVE_REQUEST);
+    }
+}
+
+static void __not_in_flash_func(i2c0_slave_irq_handler)() {
+    i2c_slave_irq_handler(&i2c_slaves[0]);
+}
+
+static void __not_in_flash_func(i2c1_slave_irq_handler)() {
+    i2c_slave_irq_handler(&i2c_slaves[1]);
+}
+
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler) {
+    assert(i2c == i2c0 || i2c == i2c1);
+    assert(handler != NULL);
+
+    uint i2c_index = i2c_hw_index(i2c);
+    i2c_slave_t *slave = &i2c_slaves[i2c_index];
+    slave->i2c = i2c;
+    slave->handler = handler;
+
+    // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
+    // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
+    // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
+    i2c_set_slave_mode(i2c, true, address);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    // unmask necessary interrupts
+    hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS | I2C_IC_INTR_MASK_M_RD_REQ_BITS | I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS | I2C_IC_INTR_MASK_M_STOP_DET_BITS | I2C_IC_INTR_MASK_M_START_DET_BITS;
+
+    // enable interrupt for current core
+    uint num = I2C0_IRQ + i2c_index;
+    irq_set_exclusive_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+    irq_set_enabled(num, true);
+}
+
+void i2c_slave_deinit(i2c_inst_t *i2c) {
+    assert(i2c == i2c0 || i2c == i2c1);
+
+    uint i2c_index = i2c_hw_index(i2c);
+    i2c_slave_t *slave = &i2c_slaves[i2c_index];
+    assert(slave->i2c == i2c); // should be called after i2c_slave_init()
+
+    slave->i2c = NULL;
+    slave->handler = NULL;
+    slave->transfer_in_progress = false;
+
+    uint num = I2C0_IRQ + i2c_index;
+    irq_set_enabled(num, false);
+    irq_remove_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    hw->intr_mask = I2C_IC_INTR_MASK_RESET;
+
+    i2c_set_slave_mode(i2c, false, 0);
+}

i2c_slave.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef _I2C_SLAVE_H_
+#define _I2C_SLAVE_H_
+
+#include <hardware/i2c.h>
+#include <pico/stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file i2c_slave.h
+ *
+ * \brief I2C slave setup.
+ */
+
+/**
+ * \brief I2C slave event types.
+ */
+typedef enum i2c_slave_event_t
+{
+    I2C_SLAVE_RECEIVE, /**< Data from master is available for reading. Slave must read from Rx FIFO. */
+    I2C_SLAVE_REQUEST, /**< Master is requesting data. Slave must write into Tx FIFO. */
+    I2C_SLAVE_FINISH, /**< Master has sent a Stop or Restart signal. Slave may prepare for the next transfer. */
+} i2c_slave_event_t;
+
+/**
+ * \brief I2C slave event handler
+ * 
+ * The event handler will run from the I2C ISR, so it should return quickly (under 25 us at 400 kb/s).
+ * Avoid blocking inside the handler and split large data transfers across multiple calls for best results.
+ * When sending data to master, up to `i2c_get_write_available()` bytes can be written without blocking.
+ * When receiving data from master, up to `i2c_get_read_available()` bytes can be read without blocking.
+ * 
+ * \param i2c Slave I2C instance.
+ * \param event Event type.
+ */
+typedef void (*i2c_slave_handler_t)(i2c_inst_t *i2c, i2c_slave_event_t event);
+
+/**
+ * \brief Configure I2C instance for slave mode.
+ * 
+ * \param i2c I2C instance.
+ * \param address 7-bit slave address.
+ * \param handler Called on events from I2C master. It will run from the I2C ISR, on the CPU core
+ *                where the slave was initialized.
+ */
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler);
+
+/**
+ * \brief Restore I2C instance to master mode.
+ *
+ * \param i2c I2C instance.
+ */
+void i2c_slave_deinit(i2c_inst_t *i2c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _I2C_SLAVE_H_

main.c

@@ -1,16 +1,101 @@
-/*****
- * Copyright (c) 2023 - Poivron Robotique
- *
- * SPDX-License-Identifier: BSD-3-Clause
-*/
 #include "pico/stdlib.h"
+#include "hardware/adc.h"
+#include "communication.h"
+#include "Servomoteur.h"
 #include <stdio.h>
+#include "moteur.h"
+
+// Juste pour information - les broches sont re-définies dans i2c_slave
+#define I2C0_SDA_PIN 16
+#define I2C0_SCL_PIN 17
+
+// Juste pour information - les broches sont re-définies dans i2c_master
+#define I2C1_SDA_PIN 18
+#define I2C1_SCL_PIN 19
+
+// Allumage de la led verte
+#define PIN_LED_VERTE 25
 
 void main(void)
 {
+	char message[256], reception[256];
+	
+	// Init "all"
     stdio_init_all();
+	Init_motion_motor();
+	Servomoteur_init();
+	communication_init();
+	
+	gpio_init(PIN_LED_VERTE);
+	gpio_set_function(PIN_LED_VERTE, GPIO_FUNC_PWM);
+	pwm_set_clkdiv(4, 100);
+	pwm_set_wrap(4, 100);
+	pwm_set_enabled(4, true);
+	pwm_set_chan_level(4, PWM_CHAN_B, 100);
+
+	int vitesse_angle = 0;
+	int vitesse_m1 = 0;
+	int vitesse_m2 = 0;
+	int vitesse_m3 = 0;
+	int vitesse_m4 = 0;
+
 	while(1){
-		printf("Exemple\n");
-		sleep_ms(1000);
+
+		if (!(communication_read_message(reception) == I2C_ECHEC))
+		{
+			vitesse_m1 = reception[0] - 128;
+			vitesse_m2 = reception[0] - 128;
+			vitesse_m3 = reception[1] - 128;
+			vitesse_m4 = reception[1] - 128;
+
+			vitesse_angle = reception[2] - 128;
+
+			switch(reception[3]){
+				case 0: Servo_pince_lache(); break;
+				case 1: Servo_pince_tient(); break;
+			}
+
+			switch(reception[4]){
+				case 0: Servo_plie_banderole(); break;
+				case 1: Servo_deplie_banderole(); break;
+			}
+			switch(reception[5]){
+				case 0: Servo_came_neutre(); break;
+				case 1: Servo_came_pousse(); break;
+				
+			}
+
+		}
+		else
+		{
+			vitesse_angle = 0;
+			vitesse_m1 = 0;
+			vitesse_m2 = 0;
+			vitesse_m3 = 0;
+			vitesse_m4 = 0;
+
+			pwm_set_chan_level(4, PWM_CHAN_B, 10);
+		
+		}
+
+		if(vitesse_angle != 0)
+		{
+			vitesse_m1 = vitesse_angle;
+			vitesse_m2 = -vitesse_angle;
+			vitesse_m3 = vitesse_angle;
+			vitesse_m4 = -vitesse_angle;
+		}
+
+		printf(">vitesseM1:%d\n", vitesse_m1);
+		printf(">vitesseM2:%d\n", vitesse_m2);
+		printf(">vitesseM3:%d\n", vitesse_m3);
+		printf(">vitesseM4:%d\n", vitesse_m4);
+
+		Motor1_speed(vitesse_m1);
+		Motor2_speed(vitesse_m2);
+		Motor3_speed(vitesse_m3);
+		Motor4_speed(vitesse_m4);
+
+		
 	}
 }

moteur.c

@@ -0,0 +1,200 @@
+#include "moteur.h"
+
+// Define pins servo
+#define MOTEUR1_PIN_SENS1 4
+#define MOTEUR1_PIN_SENS2 5
+#define MOTEUR1_PIN_ACTIVATION 0
+#define MOTEUR2_PIN_SENS1 6
+#define MOTEUR2_PIN_SENS2 7
+#define MOTEUR2_PIN_ACTIVATION 1
+#define MOTEUR3_PIN_SENS1 11
+#define MOTEUR3_PIN_SENS2 10
+#define MOTEUR3_PIN_ACTIVATION 2
+#define MOTEUR4_PIN_SENS1 8
+#define MOTEUR4_PIN_SENS2 9
+#define MOTEUR4_PIN_ACTIVATION 3
+
+// Init all motion motors pins
+void Init_motion_motor(void)
+{
+	// Init motion motors
+
+	// Init 1/0 pin for control motion motors
+	gpio_init(MOTEUR1_PIN_SENS1);
+	gpio_init(MOTEUR1_PIN_SENS2);
+	gpio_init(MOTEUR2_PIN_SENS1);
+	gpio_init(MOTEUR2_PIN_SENS2);
+	gpio_init(MOTEUR3_PIN_SENS1);
+	gpio_init(MOTEUR3_PIN_SENS2);
+	gpio_init(MOTEUR4_PIN_SENS1);
+	gpio_init(MOTEUR4_PIN_SENS2);
+	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR3_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR3_PIN_SENS2, GPIO_OUT);
+	gpio_set_dir(MOTEUR4_PIN_SENS1, GPIO_OUT);
+	gpio_set_dir(MOTEUR4_PIN_SENS2, GPIO_OUT);
+
+	// Set direction to 0 (disactivate)
+	gpio_put(MOTEUR1_PIN_SENS1, 0);
+	gpio_put(MOTEUR1_PIN_SENS2, 0);
+	gpio_put(MOTEUR2_PIN_SENS1, 0);
+	gpio_put(MOTEUR2_PIN_SENS2, 0);
+	gpio_put(MOTEUR3_PIN_SENS1, 0);
+	gpio_put(MOTEUR3_PIN_SENS2, 0);
+	gpio_put(MOTEUR4_PIN_SENS1, 0);
+	gpio_put(MOTEUR4_PIN_SENS2, 0);
+
+	// Init pwm pins for motion motors
+	gpio_init(MOTEUR1_PIN_ACTIVATION);
+	gpio_init(MOTEUR2_PIN_ACTIVATION);
+	gpio_init(MOTEUR3_PIN_ACTIVATION);
+	gpio_init(MOTEUR4_PIN_ACTIVATION);
+	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR3_PIN_ACTIVATION, GPIO_FUNC_PWM);
+	gpio_set_function(MOTEUR4_PIN_ACTIVATION, GPIO_FUNC_PWM);
+
+	// Set wrap of pwm slices
+	pwm_set_wrap(0, 127);
+	pwm_set_wrap(1, 127);
+
+	// Active all pwm slices
+	pwm_set_enabled(0, true);
+	pwm_set_enabled(1, true);
+
+	// Set speed to 0
+	pwm_set_chan_level(0, PWM_CHAN_A, 0);
+	pwm_set_chan_level(0, PWM_CHAN_B, 0);
+	pwm_set_chan_level(1, PWM_CHAN_A, 0);
+	pwm_set_chan_level(1, PWM_CHAN_B, 0);
+
+	
+
+	
+}
+
+
+// Set motor 1 speed forward
+void Motor1_forward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR1_PIN_SENS1, 0);
+	gpio_put(MOTEUR1_PIN_SENS2, 1);
+}
+
+// Set motor 1 speed backward
+void Motor1_backward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR1_PIN_SENS1, 1);
+	gpio_put(MOTEUR1_PIN_SENS2, 0);
+}
+
+// Set motor 1 speed and direction (negative value : backward / positive value : forward)
+void Motor1_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor1_backward(speed);
+	}
+	else
+	{
+		Motor1_forward(speed);
+	}
+}
+
+// Set motor 2 speed forward
+void Motor2_forward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR2_PIN_SENS1, 0);
+	gpio_put(MOTEUR2_PIN_SENS2, 1);
+}
+
+// Set motor 2 speed backward
+void Motor2_backward(int speed)
+{
+	pwm_set_chan_level(0, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR2_PIN_SENS1, 1);
+	gpio_put(MOTEUR2_PIN_SENS2, 0);
+}
+
+// Set motor 2 speed and direction (negative value : backward / positive value : forward)
+void Motor2_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor2_backward(speed);
+	}
+	else
+	{
+		Motor2_forward(speed);
+	}
+}
+
+// Set motor 3 speed forward
+void Motor3_forward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR3_PIN_SENS1, 0);
+	gpio_put(MOTEUR3_PIN_SENS2, 1);
+}
+
+// Set motor 3 speed backward
+void Motor3_backward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_A, speed);
+	gpio_put(MOTEUR3_PIN_SENS1, 1);
+	gpio_put(MOTEUR3_PIN_SENS2, 0);
+}
+
+// Set motor 3 speed and direction (negative value : backward / positive value : forward)
+void Motor3_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor3_backward(speed);
+	}
+	else
+	{
+		Motor3_forward(speed);
+	}
+}
+
+// Set motor 4 speed forward
+void Motor4_forward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR4_PIN_SENS1, 1);
+	gpio_put(MOTEUR4_PIN_SENS2, 0);
+}
+
+// Set motor 4 speed backward
+void Motor4_backward(int speed)
+{
+	pwm_set_chan_level(1, PWM_CHAN_B, speed);
+	gpio_put(MOTEUR4_PIN_SENS1, 0);
+	gpio_put(MOTEUR4_PIN_SENS2, 1);
+}
+
+// Set motor 4 speed and direction (negative value : backward / positive value : forward)
+void Motor4_speed(int speed)
+{
+	if(speed < 0)
+	{
+		speed = -speed;
+		Motor4_backward(speed);
+	}
+	else
+	{
+		Motor4_forward(speed);
+	}
+}
+
+// Set

moteur.h

@@ -0,0 +1,17 @@
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+
+void Init_motion_motor(void);
+void Motor1_forward(int speed);
+void Motor1_backward(int speed);
+void Motor1_speed(int speed);
+void Motor2_forward(int speed);
+void Motor2_backward(int speed);
+void Motor2_speed(int speed);
+void Motor3_forward(int speed);
+void Motor3_backward(int speed);
+void Motor3_speed(int speed);
+void Motor2_forward(int speed);
+void Motor3_backward(int speed);
+void Motor4_speed(int speed);
+