Tiens compte du cas où le capteur voit le gradin sans voir le bord

.vscode/c_cpp_properties.json

@@ -3,6 +3,8 @@
         "myDefaultIncludePath": [
             "${workspaceFolder}",
             "${workspaceFolder}/build",
+            "${workspaceFolder}/Platform/",
+            "${workspaceFolder}/VL53L8CX_ULD_API/inc",
             "${env:PICO_SDK_PATH}/src/**/include",
             "${env:PICO_SDK_PATH}/src/common/pico_base/include",
             "${env:PICO_SDK_PATH}/build/generated/pico_base",

.vscode/settings.json

@@ -9,6 +9,7 @@
         "trajet.h": "c",
         "trajectoire.h": "c",
         "compare": "c",
-        "asser_position.h": "c"
+        "asser_position.h": "c",
+        "vl53l8cx_api.h": "c"
     }
 }

Asser_Moteurs.c

@@ -1,90 +0,0 @@
-#include "QEI.h"
-#include "Moteurs.h"
-#include "Asser_Moteurs.h"
-
-#define ASSERMOTEUR_GAIN_P 300000.f
-#define ASSERMOTEUR_GAIN_I 30000.f
-
-float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
-float commande_I[3]; // Terme integral
-
-void AsserMoteur_Init(int id){
-    QEI_init(id);
-    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

@@ -1,8 +0,0 @@
-#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(int);

Asser_Position.c

@@ -1,63 +0,0 @@
-#include "Localisation.h"
-#include "Commande_vitesse.h"
-#include "math.h"
-
-#define GAIN_P_POSITION 15
-#define GAIN_P_ORIENTATION 10
-
-struct position_t position_maintien;
-
-/// @brief Asservissement de la position du robot. Les gains sont déterminés pour des positions très proches du robot
-/// C'est à la consigne d'être défini avant pour être atteignable.
-/// Nécessite l'appel des fonctions QEI_update(); Localisation_gestion(); AsserMoteur_Gestion(_step_ms);
-/// @param position_consigne : position à atteindre dans le référentiel de la table.
-float delta_x_mm, delta_y_mm, delta_orientation_radian;
-float delta_orientation_radian_tmp;
-void Asser_Position(struct position_t position_consigne){
-    float delta_avance_mm;
-    float avance_mm_s, rotation_radian_s;
-    //float delta_x_mm, delta_y_mm, delta_orientation_radian;
-    struct position_t position_actuelle;
-    float delta_orientation_radian_tmp;
-
-    position_actuelle = Localisation_get();
-
-    // Calcul de l'erreur
-    delta_x_mm = position_consigne.x_mm - position_actuelle.x_mm;
-    delta_y_mm = position_consigne.y_mm - position_actuelle.y_mm;
-    delta_avance_mm = sqrtf(delta_x_mm * delta_x_mm + delta_y_mm * delta_y_mm);
-    delta_orientation_radian_tmp = atan2f(delta_y_mm, delta_x_mm) - position_actuelle.angle_radian;
-    delta_orientation_radian = Geometrie_get_angle_optimal(0. , delta_orientation_radian_tmp);
-    
-    // On asservi sur +PI/2 / -PI/2
-    if(delta_orientation_radian > (M_PI/2)){
-        delta_orientation_radian -= M_PI;
-        delta_avance_mm = -delta_avance_mm;
-    }
-    if(delta_orientation_radian < -(M_PI/2)){
-        delta_orientation_radian += M_PI;
-        delta_avance_mm = -delta_avance_mm;
-    }
-    
-
-
-    // Asservissement
-    avance_mm_s = delta_avance_mm * GAIN_P_POSITION;
-    rotation_radian_s = delta_orientation_radian * GAIN_P_ORIENTATION;
-
-    if(delta_avance_mm < 10){
-        rotation_radian_s=0;
-    }
-    
-    // Commande en vitesse
-    commande_vitesse(avance_mm_s, rotation_radian_s);
-
-}
-
-void Asser_Position_set_Pos_Maintien(struct position_t position){
-    position_maintien=position;
-}
-
-void Asser_Position_maintien(){
-    Asser_Position(position_maintien);
-}

Asser_Position.h

@@ -1,4 +0,0 @@
-#include "Geometrie.h"
-void Asser_Position(struct position_t position_consigne);
-void Asser_Position_set_Pos_Maintien(struct position_t position);
-void Asser_Position_maintien();

CMakeLists.txt

@@ -2,57 +2,49 @@ cmake_minimum_required(VERSION 3.13)
 
 include(pico_sdk_import.cmake)
 
-project(Mon_Projet C CXX ASM)
+project(VL53L8X_Gradin C CXX ASM)
 set(CMAKE_C_STNDARD 11)
 set(CMAKE_CXX_STANDARD 17)
 
 set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
 
+set(PICO_BOARD seeed_xiao_rp2040)
+
 pico_sdk_init()
 
-add_executable(Mon_Projet
+add_executable(VL53L8X_Gradin
-  Asser_Position.c
-  Asser_Moteurs.c
-  Commande_vitesse.c
   Geometrie.c
-  i2c_maitre.c
+  i2c_slave.c
-  Moteurs.c
-  Localisation.c
   main.c
-  QEI.c
   Temps.c
-  Trajectoire_bezier.c
-  Trajectoire_circulaire.c
-  Trajectoire_droite.c
-  Trajectoire.c
-  Trajet.c
   VL53L8CX_ULD_API/src/vl53l8cx_api.c
   VL53L8CX_ULD_API/src/vl53l8cx_plugin_detection_thresholds.c
   VL53L8CX_ULD_API/src/vl53l8cx_plugin_motion_indicator.c
   VL53L8CX_ULD_API/src/vl53l8cx_plugin_xtalk.c
   VL53L8_2024.c
   Platform/platform.c
+  ws2812.c
 )
+# generate the header file into the source tree as it is included in the RP2040 datasheet
+pico_generate_pio_header(VL53L8X_Gradin ${CMAKE_CURRENT_LIST_DIR}/ws2812.pio)
 
-pico_generate_pio_header(Mon_Projet ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+target_include_directories(VL53L8X_Gradin PRIVATE VL53L8CX_ULD_API/inc/)
 
-target_include_directories(Mon_Projet PRIVATE VL53L8CX_ULD_API/inc/)
+target_link_libraries(VL53L8X_Gradin
-
-target_link_libraries(Mon_Projet
   hardware_adc 
+  hardware_spi
   hardware_i2c
-  hardware_pwm 
   hardware_pio
   pico_stdlib
   pico_multicore
 )
 
-pico_enable_stdio_usb(Mon_Projet 1)
+pico_enable_stdio_usb(VL53L8X_Gradin 1)
-pico_enable_stdio_uart(Mon_Projet 1)
+pico_enable_stdio_uart(VL53L8X_Gradin 1)
 
-pico_add_extra_outputs(Mon_Projet)
+pico_add_extra_outputs(VL53L8X_Gradin)
 
 add_custom_target(Flash
-    DEPENDS Mon_Projet
+    DEPENDS VL53L8X_Gradin
-    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/VL53L8X_Gradin.uf2
 )

Commande_vitesse.c

@@ -1,27 +0,0 @@
-#include "Asser_Moteurs.h"
-#include "Geometrie_robot.h"
-#include "Commande_vitesse.h"
-
-
-
-
-
-/// @brief Commande de la vitesse dans le référentiel du robot
-/// @param avance_mm_s : Vitesse d'avance
-/// @param orientation_radian_s : Rotation en radian/s 
-void commande_vitesse(float avance_mm_s, float orientation_radian_s){
-    float vitesse_roue_gauche, vitesse_roue_droite;
-
-    vitesse_roue_gauche = avance_mm_s - (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);
-    vitesse_roue_droite = avance_mm_s + (orientation_radian_s * DISTANCE_ROUES_CENTRE_MM);
-
-    AsserMoteur_setConsigne_mm_s(MOTEUR_A, vitesse_roue_droite);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_gauche);
-
-}
-
-/// @brief Arrête le robot.
-void commande_vitesse_stop(){
-    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
-    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0);
-}

Commande_vitesse.h

@@ -1,2 +0,0 @@
-void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s);
-void commande_vitesse_stop(void);

Localisation.c

@@ -1,54 +0,0 @@
-#include "Localisation.h"
-#include "Temps.h"
-#include "QEI.h"
-#include "math.h"
-#include "Geometrie_robot.h"
-
-struct position_t position;
-
-void Localisation_init(int id){
-    Temps_init();
-    QEI_init(id);
-    position.x_mm = 0;
-    position.y_mm = 0;
-    position.angle_radian = 0;
-}
-
-void Localisation_set(float x_mm, float y_mm, float angle_radian){
-    position.x_mm = x_mm;
-    position.y_mm = y_mm;
-    position.angle_radian = angle_radian;
-}
-
-void Localisation_set_x(float x_mm){
-    position.x_mm = x_mm;
-}
-
-void Localisation_set_y(float y_mm){
-    position.y_mm = y_mm;
-}
-
-void Localisation_set_angle(float angle_radian){
-    position.angle_radian = angle_radian;
-}
-
-void Localisation_gestion(){
-    float distance_roue_gauche_mm = QEI_get_mm(QEI_B_NAME);
-    float distance_roue_droite_mm = QEI_get_mm(QEI_A_NAME);
-    
-    float delta_avance_mm, delta_orientation_rad;
-
-    delta_avance_mm = (distance_roue_droite_mm + distance_roue_gauche_mm)/2;
-    delta_orientation_rad = (distance_roue_droite_mm - distance_roue_gauche_mm) / (DISTANCE_ROUES_CENTRE_MM*2);
-
-    position.angle_radian += delta_orientation_rad;
-
-    // Projection dans le référentiel de la table
-    position.x_mm += delta_avance_mm * cosf(position.angle_radian);
-    position.y_mm += delta_avance_mm * sinf(position.angle_radian);
-
-}
-
-struct position_t Localisation_get(void){
-    return position;
-}

Localisation.h

@@ -1,10 +0,0 @@
-#include "Geometrie.h"
-
-struct position_t Localisation_get(void);
-void Localisation_gestion();
-void Localisation_init(int);
-
-void Localisation_set(float x_mm, float y_mm, float angle_radian);
-void Localisation_set_x(float x_mm);
-void Localisation_set_y(float y_mm);
-void Localisation_set_angle(float angle_radian);

Moteurs.c

@@ -1,98 +0,0 @@
-#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

@@ -1,14 +0,0 @@
-#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);

Platform/platform.c

@@ -13,101 +13,63 @@
 
 #include "platform.h"
 #include "pico/stdlib.h"
-#include "hardware/i2c.h"
+#include "hardware/spi.h"
 #include "hardware/gpio.h"
 #include <stdio.h>
+#include "vl53l8cx_api.h"
 
-#define I2C_SUCCESS 0
+#define BUFFER_SIZE 0x1000
-#define I2C_FAILED 1
+#define ERR_OK 0
-#define I2C_BUFFER_EXCEEDED 2
 
-#define I2C_DEVICE i2c1
+spi_inst_t * my_spi;
 
-#define MAX_I2C_BUFFER 0x8100
+void SPI_beginTransaction(char my_ss_pin){
-
+  /* switch(my_ss_pin){
-
+    case D0: SPI.begin(SCK, MISO, MOSI, SS); break;
-/// @brief Blocking function allowing to write a register on an I2C device
+    case D1: SPI.begin(SCK, D3, MOSI, SS); break;
-/// @param address_7_bits 
+  }*/
-/// @param index : register to write
+  // 
-/// @param values : values to write
+  gpio_put(my_ss_pin, 0);
-/// @param count : number of byte to send
+  if(my_ss_pin == 1){
-/// @return 0: Success, -1 or -2: Failed 
+    my_spi = spi0;
-int8_t i2c_write_register(char adresse_7_bits, uint16_t index, uint8_t * values, uint32_t count){
+  }else{
-    int statu;
+    my_spi = spi1;
-	uint8_t buffer[MAX_I2C_BUFFER];
-    uint8_t index_to_unint8[2];
-	absolute_time_t timeout_time;
-
-	if(count > MAX_I2C_BUFFER - 2){
-		return I2C_BUFFER_EXCEEDED;
   }
   
-	index_to_unint8[0] =  (index >> 8) & 0xFF;
-	index_to_unint8[1] =  index & 0xFF;
-
-	buffer[0] = index_to_unint8[0];
-	buffer[1] = index_to_unint8[1];
-	
-	for(uint32_t i=0; i<count; i++){
-		buffer[2+i] = values[i];
 }
 
-	// Define timeout - now + 1s.
+void SPI_endTransaction(char my_ss_pin){
-	timeout_time = time_us_64() + 5000000;
+  //SPI.endTransaction();
-    statu = i2c_write_blocking_until (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0, timeout_time);
+  gpio_put(my_ss_pin, 1); 
-	//statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0);
+  //SPI.end();
-    if(statu == PICO_ERROR_GENERIC){
-        printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits);
-        return I2C_FAILED;
-    }else if(statu == PICO_ERROR_TIMEOUT){
-		printf("Erreur ecrire registre: timeout\n");
-		printf("Adresse : %x\n",adresse_7_bits << 1 );
-		return I2C_FAILED;
 }
 
-    return I2C_SUCCESS;
-}
-
-
-/// @brief Blocking function allowing to write a register on an I2C device
-/// @param address_7_bits 
-/// @param index : register to write
-/// @param values : values to write
-/// @param count : number of byte to send
-/// @return 0: Success, -1 or -2: Failed 
-int8_t i2c_read_register(char adresse_7_bits, uint16_t index, uint8_t *pdata, uint32_t count){
-    int statu;
-	uint8_t buffer[MAX_I2C_BUFFER];
-    uint8_t index_to_unint8[2];
-
-	index_to_unint8[0] =  (index >> 8) & 0xFF;
-	index_to_unint8[1] =  index & 0xFF;
-
-    statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, index_to_unint8, 2, 0);
-    if(statu == PICO_ERROR_GENERIC){
-        printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits);
-        return I2C_FAILED;
-    }
-
-    statu = i2c_read_blocking (I2C_DEVICE, adresse_7_bits, pdata, count, 0);
-    if(statu == PICO_ERROR_GENERIC){
-        printf("I2C - Lecture registre Echec\n");
-        return I2C_FAILED;
-    }
-
-    return I2C_SUCCESS;
-}
-
-
 uint8_t RdByte(
 		VL53L8CX_Platform *p_platform,
 		uint16_t RegisterAdress,
 		uint8_t *p_value)
 {
 	uint8_t status = 255;
+  uint8_t my_ss_pin;
+  uint8_t tampon[2];
+  my_ss_pin = p_platform->address;
 	
 	/* Need to be implemented by customer. This function returns 0 if OK */
-	return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_value, 1);
+  //Serial.printf("RdByte adresse :%x\n",p_platform->address);
+  SPI_beginTransaction(my_ss_pin);
+
+  tampon[0] = RegisterAdress >> 8 & 0x7F;
+  tampon[1] = RegisterAdress & 0xFF;
+
+  spi_write_blocking(my_spi, tampon, 2);
+  spi_read_blocking(my_spi, 0x00, p_value, 1 );
+
+  //printf("RdByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], *p_value);
+
+  SPI_endTransaction(my_ss_pin);
+  
+  return ERR_OK;
+	
 }
 
 uint8_t WrByte(
@@ -116,9 +78,26 @@ uint8_t WrByte(
 		uint8_t value)
 {
 	uint8_t status = 255;
+  uint8_t my_ss_pin;
+  uint8_t tampon[3];
+  
+  my_ss_pin = p_platform->address;
 
 	/* Need to be implemented by customer. This function returns 0 if OK */
-	return i2c_write_register(p_platform->address >> 1, RegisterAdress, &value, 1);
+  SPI_beginTransaction(my_ss_pin);
+
+  tampon[0] = RegisterAdress >> 8 | 0x80;
+  tampon[1] = RegisterAdress & 0xFF;
+  tampon[2] = value;
+
+  //printf("WrByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], tampon[2]);
+
+  spi_write_blocking(my_spi, tampon, 3);
+   
+  SPI_endTransaction(my_ss_pin);
+  
+
+  return ERR_OK;
 
 }
 
@@ -128,9 +107,32 @@ uint8_t WrMulti(
 		uint8_t *p_values,
 		uint32_t size)
 {
-	uint8_t status = 255;
+  uint32_t index =0;
+  uint8_t my_buffer[BUFFER_SIZE];
+  uint8_t tampon[2];
+  uint8_t my_ss_pin;
+  my_ss_pin = p_platform->address;
 
-	return i2c_write_register(p_platform->address >> 1, RegisterAdress, p_values, size);
+  SPI_beginTransaction(my_ss_pin);
+  
+  tampon[0] = RegisterAdress >> 8 | 0x80;
+  tampon[1] = RegisterAdress & 0xFF;
+  spi_write_blocking(my_spi, tampon, 2);
+
+  while(size > BUFFER_SIZE){
+    memcpy(my_buffer, (uint8_t*)&p_values[index], BUFFER_SIZE);
+    spi_write_blocking(my_spi, my_buffer, BUFFER_SIZE);
+    size = size - BUFFER_SIZE;
+    index = index + BUFFER_SIZE;
+  }
+  if(size > 0){
+	memcpy(my_buffer, (uint8_t*)&p_values[index], size);
+	spi_write_blocking(my_spi, my_buffer, size);
+  }
+  
+  SPI_endTransaction(my_ss_pin);
+
+  return ERR_OK;
 
 }
 
@@ -141,8 +143,36 @@ uint8_t RdMulti(
 		uint32_t size)
 {
 	uint8_t status = 255;
+  uint32_t index =0;
+  uint32_t buffer_size = 32;
+  uint8_t my_ss_pin;
+  uint8_t tampon[2];
+  my_ss_pin = p_platform->address;
 
-	return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_values, size);
+  SPI_beginTransaction(my_ss_pin);
+  
+  tampon[0] = RegisterAdress >> 8 & 0x7F;
+  tampon[1] = RegisterAdress & 0xFF;
+
+  spi_write_blocking(my_spi, tampon, 2);
+  spi_read_blocking(my_spi, 0, p_values, size);
+
+  //printf("RdMulti 0x%02X%02X size : 0x%02X\n", tampon[0], tampon[1], size);
+
+/*
+  while(size > buffer_size){
+    spi_read_blocking(my_spi, 0, &(p_values[index]), buffer_size);
+    index += buffer_size;
+    size -= buffer_size;
+  }
+  if(size > 0){
+  	spi_read_blocking(my_spi, 0, &(p_values[index]), size);
+  }
+  */
+  
+  SPI_endTransaction(my_ss_pin);
+  
+  return ERR_OK;
 	
 }
 
@@ -193,6 +223,6 @@ uint8_t WaitMs(
 {
 	/* Need to be implemented by customer. This function returns 0 if OK */
 	sleep_ms(TimeMs);
-	
+	//delay(TimeMs);
 	return 0;
 }

QEI.c

@@ -1,101 +0,0 @@
-#include <stdio.h>
-#include "pico/stdlib.h"
-#include "hardware/pio.h"
-#include "hardware/timer.h"
-#include "QEI.h"
-#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_50_1 (12.45f)
-#define IMPULSION_PAR_MM_30_1 (7.47f)
-
-float impulsion_par_mm;
-
-
-struct QEI_t QEI_A, QEI_B;
-
-bool QEI_est_init = false;
-
-PIO pio_QEI = pio0;
-
-
-void QEI_init(int identifiant){
-    // Initialisation des 3 modules QEI
-    // Chaque module QEI sera dans une machine à état du PIO 0
-    if(!QEI_est_init){
-        // Offset le début du programme
-        // Si ce n'est pas 0, le programme ne marchera pas
-        uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); 
-        if(offset != 0){
-            printf("PIO init error: offset != 0");
-        }
-        // Initialisation des "machines à états" :
-        // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer
-        quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0);
-        // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer
-        quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);
-
-        QEI_A.value=0;
-        QEI_B.value=0;
-        QEI_est_init=true;
-    }
-    impulsion_par_mm = IMPULSION_PAR_MM_50_1;
-    if(identifiant == 0 || identifiant >= 4){
-        impulsion_par_mm = IMPULSION_PAR_MM_30_1;
-    }
-
-}
-
-/// @brief Lit les modules QEI et stock l'écart en cette lecture et la lecture précédente.
-void QEI_update(void){
-    
-    int 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_B.value;
-    QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1);
-    QEI_B.delta = QEI_B.value - old_value;
-
-}
-
-/// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente
-/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation.
-/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique
-/// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
-/// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update()
-int QEI_get(enum QEI_name_t qei){
-    switch (qei)
-    {
-    case QEI_A_NAME:
-        return QEI_A.delta;
-        break;
-
-    case QEI_B_NAME:
-        return -QEI_B.delta;
-        break;
-    
-    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()
-float QEI_get_mm(enum QEI_name_t qei){
-    return ((float) QEI_get(qei)) / impulsion_par_mm;
-}

QEI.h

@@ -1,16 +0,0 @@
-struct QEI_t{
-    int value;
-    int delta;
-};
-
-enum QEI_name_t{
-    QEI_A_NAME=0,
-    QEI_B_NAME=1,
-};
-
-extern struct QEI_t QEI_A, QEI_B, QEI_C;
-
-void QEI_update(void);
-void QEI_init(int);
-int QEI_get(enum QEI_name_t qei);
-float QEI_get_mm(enum QEI_name_t qei);

Readme.md

@@ -3,7 +3,7 @@ PAMI 2024 - Poivron Robotique
 
 Code du PAMI 2024 de l'équipe Poivron Robotique.
 
-La cart e contien les éléments suivants :
+La carte contient les éléments suivants :
 
 *    Microcontrôleur Raspberry Pi Pico
 *    Connecteur pour l’arrêt d’urgence

Trajectoire.c

@@ -1,167 +0,0 @@
-#include "Trajectoire.h"
-#include "Trajectoire_bezier.h"
-#include "Trajectoire_circulaire.h"
-#include "Trajectoire_droite.h"
-
-#include "math.h"
-
-#define NB_MAX_TRAJECTOIRES 5
-#define PRECISION_ABSCISSE 0.001f
-
-
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre, float rayon,
-                        float orientation_debut_rad, float orientation_fin_rad){
-    trajectoire->type = TRAJECTOIRE_CIRCULAIRE;
-    trajectoire->p1.x = centre_x;
-    trajectoire->p1.y = centre_y;
-    trajectoire->angle_debut_degre = angle_debut_degre;
-    trajectoire->angle_fin_degre = angle_fin_degre;
-    trajectoire->rayon = rayon;
-    trajectoire->longueur = -1;
-    trajectoire->orientation_debut_rad = orientation_debut_rad;
-    trajectoire->orientation_fin_rad = orientation_fin_rad;
-}
-
-void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y,
-                        float orientation_debut_rad, float orientation_fin_rad){
-    trajectoire->type = TRAJECTOIRE_DROITE;
-    trajectoire->p1.x = p1_x;
-    trajectoire->p1.y = p1_y;
-    trajectoire->p2.x = p2_x;
-    trajectoire->p2.y = p2_y;
-    trajectoire->longueur = -1;
-    trajectoire->orientation_debut_rad = orientation_debut_rad;
-    trajectoire->orientation_fin_rad = orientation_fin_rad;
-}
-
-void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,
-                        float orientation_debut_rad, float orientation_fin_rad){
-    trajectoire->type = TRAJECTOIRE_BEZIER;
-    trajectoire->p1.x = p1_x;
-    trajectoire->p1.y = p1_y;
-    trajectoire->p2.x = p2_x;
-    trajectoire->p2.y = p2_y;
-    trajectoire->p3.x = p3_x;
-    trajectoire->p3.y = p3_y;
-    trajectoire->p4.x = p4_x;
-    trajectoire->p4.y = p4_y;
-    trajectoire->longueur = -1;
-    trajectoire->orientation_debut_rad = orientation_debut_rad;
-    trajectoire->orientation_fin_rad = orientation_fin_rad;
-}
-
-void Trajectoire_inverse(struct trajectoire_t * trajectoire){
-    struct trajectoire_t old_trajectoire;
-    old_trajectoire = *trajectoire;
-
-    trajectoire->orientation_debut_rad = old_trajectoire.orientation_fin_rad;
-    trajectoire->orientation_fin_rad = old_trajectoire.orientation_debut_rad;
-
-    if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE){
-        trajectoire->angle_debut_degre = old_trajectoire.angle_fin_degre;
-        trajectoire->angle_fin_degre = old_trajectoire.angle_debut_degre;
-        return;
-    }
-    if(trajectoire->type == TRAJECTOIRE_DROITE){
-        trajectoire->p1 = old_trajectoire.p2;
-        trajectoire->p2 = old_trajectoire.p1;
-        return;
-    }
-    if(trajectoire->type == TRAJECTOIRE_BEZIER){
-        trajectoire->p1 = old_trajectoire.p4;
-        trajectoire->p2 = old_trajectoire.p3;
-        trajectoire->p3 = old_trajectoire.p2;
-        trajectoire->p4 = old_trajectoire.p1;
-    }
-
-
-
-}
-
-/// @brief Renvoie la longueur de la trajectoire en mm, la calcule si besoin
-/// @param trajectoire 
-/// @return Longueur de la trajectoire
-float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire){
-    if(trajectoire->longueur > 0){
-        // La longueur est déjà calculée
-    }else{
-        // Calculons la longueur de la trajectoire
-        switch(trajectoire->type){
-            case TRAJECTOIRE_DROITE:
-                Trajectoire_droite_get_longueur(trajectoire);
-                break;
-            case TRAJECTOIRE_CIRCULAIRE:
-                Trajectoire_circulaire_get_longueur(trajectoire);
-                break;
-            
-            case TRAJECTOIRE_BEZIER:
-                Trajectoire_bezier_get_longueur(trajectoire);
-                break;
-        }
-    }
-    return trajectoire->longueur;
-}
-
-/// @brief Renvoie le point d'une trajectoire à partir de son abscisse
-/// @param abscisse : abscisse sur la trajectoire
-/// @return point en coordonnées X/Y
-struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse){
-    struct point_xyo_t point_xyo;
-    switch(trajectoire->type){
-        case TRAJECTOIRE_DROITE:
-            point_xyo.point_xy = Trajectoire_droite_get_point(trajectoire, abscisse);
-            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
-            break;
-
-        case TRAJECTOIRE_CIRCULAIRE:
-            point_xyo.point_xy = Trajectoire_circulaire_get_point(trajectoire, abscisse);
-            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
-            break;
-        
-        case TRAJECTOIRE_BEZIER:
-            point_xyo.point_xy = Trajectoire_bezier_get_point(trajectoire, abscisse);
-            point_xyo.orientation = Trajectoire_get_orientation_rad(trajectoire, abscisse);
-            break;
-
-    }
-    return point_xyo;    
-}
-
-float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse){
-    return (float) trajectoire->orientation_debut_rad * (1-abscisse) + (float) trajectoire->orientation_fin_rad * abscisse;
-}
-
-/// @brief Calcul la nouvelle abscisse une fois avancé de la distance indiquée
-/// @param abscisse : Valeur entre 0 et 1, position actuelle du robot sur sa trajectoire
-/// @param distance_mm : Distance en mm de laquelle le robot doit avancer sur la trajectoire
-/// @return nouvelle abscisse
-float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm){
-    double delta_abscisse, delta_mm, erreur_relative;
-
-    if(distance_mm == 0){
-        return abscisse;
-    }
-    // Ceci permet d'avoir une abscisse exact sur les trajectoires droites, les trajectoires circulaires et les rotations
-    delta_abscisse = distance_mm / Trajectoire_get_longueur_mm(trajectoire);
-    if(trajectoire->type == TRAJECTOIRE_CIRCULAIRE || trajectoire->type == TRAJECTOIRE_DROITE){
-        return abscisse + delta_abscisse;
-    }
-
-    delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
-
-    // Sur les trajectoires de bézier, il peut être nécessaire d'affiner 
-    // Les cas où l'algorythme diverge ne devraient pas se produire car distance_cm << longeur_trajectoire.
-    erreur_relative = 1 - delta_mm / distance_mm;
-    while(fabs(erreur_relative) > PRECISION_ABSCISSE){
-        delta_abscisse = delta_abscisse * distance_mm / delta_mm;
-        delta_mm = distance_points(Trajectoire_get_point(trajectoire, abscisse).point_xy, Trajectoire_get_point(trajectoire, abscisse + delta_abscisse).point_xy );
-        erreur_relative = 1 - delta_mm / distance_mm;
-    }
-    
-    return abscisse + delta_abscisse;
-}
-
-double distance_points(struct point_xy_t point, struct point_xy_t point_old){
-    return sqrt( pow(point.x - point_old.x, 2) + pow(point.y - point_old.y , 2));
-    
-}

Trajectoire.h

@@ -1,40 +0,0 @@
-#ifndef TRAJECTOIRE_H
-#define TRAJECTOIRE_H
-
-enum trajectoire_type_t{
-    TRAJECTOIRE_DROITE,
-    TRAJECTOIRE_CIRCULAIRE,
-    TRAJECTOIRE_BEZIER,
-};
-
-struct point_xy_t{
-    double x, y;
-};
-
-struct point_xyo_t{
-    struct point_xy_t point_xy;
-    float orientation;
-};
-
-struct trajectoire_t {
-    enum trajectoire_type_t type;
-    struct point_xy_t p1, p2, p3, p4;
-    float orientation_debut_rad, orientation_fin_rad;
-    float rayon, angle_debut_degre, angle_fin_degre;
-    float longueur;
-};
-
-float Trajectoire_get_longueur_mm(struct trajectoire_t * trajectoire);
-struct point_xyo_t Trajectoire_get_point(struct trajectoire_t * trajectoire, double abscisse);
-float Trajectoire_get_orientation_rad(struct trajectoire_t * trajectoire, float abscisse);
-float Trajectoire_avance(struct trajectoire_t * trajectoire, double abscisse, double distance_mm);
-double distance_points(struct point_xy_t point, struct point_xy_t point_old);
-void Trajectoire_circulaire(struct trajectoire_t * trajectoire, float centre_x, float centre_y, float angle_debut_degre, float angle_fin_degre,
-                            float rayon, float orientation_debut_rad, float orientation_fin_rad);
-void Trajectoire_droite(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float orientation_debut_rad, float orientation_fin_rad);
-void Trajectoire_bezier(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float p2_x, float p2_y, float p3_x, float p3_y, float p4_x, float p4_y,
-                        float orientation_debut_rad, float orientation_fin_rad);
-void Trajectoire_rotation(struct trajectoire_t * trajectoire, float p1_x, float p1_y, float orientation_debut_rad, float orientation_fin_rad);
-void Trajectoire_inverse(struct trajectoire_t * trajectoire);
-
-#endif

Trajectoire_bezier.c

@@ -1,35 +0,0 @@
-#include "Trajectoire.h"
-#include "Trajectoire_bezier.h"
-
-
-void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire){
-    struct point_xy_t point, point_old;
-    float nb_pas=500;
-
-    trajectoire->longueur=0;
-    point_old = trajectoire->p1;
-
-    for(float abscisse=0; abscisse<=1; abscisse += 1./nb_pas){
-        point = Trajectoire_bezier_get_point(trajectoire, abscisse);
-        trajectoire->longueur += distance_points(point, point_old);
-        point_old = point;
-    }
-}
-
-
-/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
-/// @param abscisse : compris entre 0 et 1
-struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse){
-    struct point_xy_t point;
-    point.x = (double) trajectoire->p1.x * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (double) trajectoire->p2.x * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (double) trajectoire->p3.x * abscisse * abscisse * (1-abscisse) +
-            (double) trajectoire->p4.x * abscisse * abscisse * abscisse;
-                    
-    point.y = (double) trajectoire->p1.y * (1-abscisse) * (1-abscisse) * (1-abscisse) + 
-        3 * (double) trajectoire->p2.y * abscisse * (1-abscisse) * (1-abscisse) +
-        3 * (double) trajectoire->p3.y * abscisse * abscisse * (1-abscisse) +
-            (double) trajectoire->p4.y * abscisse * abscisse * abscisse;
-
-    return point;
-}

Trajectoire_bezier.h

@@ -1,5 +0,0 @@
-#include "Trajectoire.h"
-
-
-void Trajectoire_bezier_get_longueur(struct trajectoire_t * trajectoire);
-struct point_xy_t Trajectoire_bezier_get_point(struct trajectoire_t * trajectoire, double abscisse);

Trajectoire_circulaire.c

@@ -1,26 +0,0 @@
-#include "math.h"
-#include "Trajectoire.h"
-
-
-void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire){
-    float distance_angulaire;
-    if(trajectoire->angle_debut_degre > trajectoire->angle_fin_degre){
-        distance_angulaire = trajectoire->angle_debut_degre - trajectoire->angle_fin_degre;
-    }else{
-        distance_angulaire = trajectoire->angle_fin_degre - trajectoire->angle_debut_degre;
-    }
-    trajectoire->longueur = 2. * M_PI * trajectoire->rayon * distance_angulaire / 360.;
-}
-
-/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
-/// @param abscisse : compris entre 0 et 1
-struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float abscisse){
-    struct point_xy_t point;
-    float angle_degre;
-
-    angle_degre = (float) trajectoire->angle_debut_degre * (1-abscisse) + (float) trajectoire->angle_fin_degre * abscisse;
-    point.x = trajectoire->p1.x + cos(angle_degre/180. * M_PI) * trajectoire->rayon;
-    point.y = trajectoire->p1.y + sin(angle_degre/180. * M_PI) * trajectoire->rayon;
-
-    return point;
-}

Trajectoire_circulaire.h

@@ -1,4 +0,0 @@
-#include "math.h"
-
-void Trajectoire_circulaire_get_longueur(struct trajectoire_t * trajectoire);
-struct point_xy_t Trajectoire_circulaire_get_point(struct trajectoire_t * trajectoire, float avancement);

Trajectoire_droite.c

@@ -1,17 +0,0 @@
-#include "Trajectoire.h"
-
-
-void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire){
-    trajectoire->longueur = distance_points(trajectoire->p1, trajectoire->p2);
-}
-
-/// @brief Retourne le point sur la trajectoire en fonction de l'abscisse
-/// @param abscisse : compris entre 0 et 1
-struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse){
-    struct point_xy_t point;
-
-    point.x = (float) trajectoire->p1.x * (1. - abscisse) + (float) trajectoire->p2.x * abscisse;
-    point.y = (float) trajectoire->p1.y * (1. - abscisse) + (float) trajectoire->p2.y * abscisse;
-    
-    return point;
-}

Trajectoire_droite.h

@@ -1,4 +0,0 @@
-#include "Trajectoire.h"
-
-void Trajectoire_droite_get_longueur(struct trajectoire_t * trajectoire);
-struct point_xy_t Trajectoire_droite_get_point(struct trajectoire_t * trajectoire, float abscisse);

Trajet.c

@@ -1,220 +0,0 @@
-#include <math.h>
-#include "Geometrie.h"
-#include "Trajectoire.h"
-#include "Trajet.h"
-#include "Asser_Position.h"
-#include "Asser_Moteurs.h"
-#include "Temps.h"
-
-float Trajet_calcul_vitesse(float temps_s);
-int Trajet_terminee(float abscisse);
-
-float abscisse;    // Position entre 0 et 1 sur la trajectoire
-float position_mm; // Position en mm sur la trajectoire
-float vitesse_mm_s;
-float vitesse_max_trajet_mm_s=500;
-float acceleration_mm_ss;
-const float acceleration_mm_ss_obstacle = 500;
-struct trajectoire_t trajet_trajectoire;
-struct position_t position_consigne;
-
-float distance_obstacle_mm;
-float distance_fin_trajectoire_mm;
-const float distance_pas_obstacle = 2000;
-
-float vitesse_max_contrainte_obstacle;
-
-/// @brief Initialise le module Trajet. A appeler en phase d'initilisation
-void Trajet_init(int id){
-    Temps_init();
-    AsserMoteur_Init(id);
-    abscisse = 0;
-    vitesse_mm_s = 0;
-    position_mm = 0;
-    Trajet_config(TRAJECT_CONFIG_STD);
-}
-
-/// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets
-/// @param _vitesse_max_trajet_mm_s 
-/// @param _acceleration_mm_ss 
-void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss){
-    vitesse_max_trajet_mm_s = _vitesse_max_trajet_mm_s;
-    acceleration_mm_ss = _acceleration_mm_ss;
-}
-
-void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
-    abscisse = 0;
-    vitesse_mm_s = 0;
-    position_mm = 0;
-    trajet_trajectoire = trajectoire;
-    Trajet_set_obstacle_mm(DISTANCE_INVALIDE);
-}
-
-/// @brief Avance la consigne de position sur la trajectoire
-/// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde 
-/// @return TRAJET_EN_COURS ou TRAJET_TERMINE
-struct point_xyo_t point;
-enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
-    float distance_mm;
-    enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;
-    
-    struct position_t position;
-
-    // Calcul de la vitesse 
-    vitesse_mm_s = Trajet_calcul_vitesse(pas_de_temps_s);
-
-    // Calcul de l'avancement en mm
-    distance_mm = vitesse_mm_s * pas_de_temps_s;
-    position_mm += distance_mm;
-
-    // Calcul de l'abscisse sur la trajectoire
-    abscisse = Trajectoire_avance(&trajet_trajectoire, abscisse, distance_mm);
-    //set_debug_varf(abscisse);
-    
-    // Obtention du point consigne
-    point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
-
-    position.x_mm = point.point_xy.x;
-    position.y_mm = point.point_xy.y;
-    position.angle_radian = point.orientation;
-
-    position_consigne=position;
-    Asser_Position(position);
-
-    if(Trajet_terminee(abscisse)){
-        Asser_Position_set_Pos_Maintien(position);
-        trajet_etat = TRAJET_TERMINE;
-    }
-    return trajet_etat;
-
-}
-
-void Trajet_stop(float pas_de_temps_s){
-    vitesse_mm_s = 0;
-    Trajet_avance(0);
-}
-
-/// @brief Savoir si un trajet est terminé est trivial sauf pour les courbes de Bézier
-/// où les approximations font que l'abscisse peut ne pas atteindre 1.
-/// @param abscisse : abscisse sur la trajectoire
-/// @return 1 si le trajet est terminé, 0 sinon
-int Trajet_terminee(float abscisse){
-    /*if(abscisse >= 0.99 ){
-        return 1;
-    }*/
-    
-    if(trajet_trajectoire.type != TRAJECTOIRE_BEZIER){
-        if(abscisse >= 1 || distance_fin_trajectoire_mm < 0.1){
-            return 1;
-        }
-    }else{
-        if(abscisse >= 0.99 ){
-            return 1;
-        }
-    }
-    return 0;
-}
-
-/// @brief Envoie la consigne de position calculée par le module trajet. Principalement pour le débug/réglage asservissement.
-struct position_t Trajet_get_consigne(){
-    return position_consigne;
-}
-
-/// @brief Calcule la vitesse à partir de l’accélération du robot, de la vitesse maximale et de la contrainte en fin de trajectoire
-/// @param pas_de_temps_s : temps écoulé en ms
-/// @return vitesse déterminée en m/s
-float Trajet_calcul_vitesse(float pas_de_temps_s){
-    float vitesse_max_contrainte;
-    float distance_contrainte,distance_contrainte_obstacle;
-    float vitesse;
-    // Calcul de la vitesse avec acceleration
-    vitesse = vitesse_mm_s + acceleration_mm_ss * pas_de_temps_s;
-
-    // Calcul de la vitesse maximale due à la contrainte en fin de trajectoire (0 mm/s)
-    // https://poivron-robotique.fr/Consigne-de-vitesse.html
-    distance_contrainte = Trajectoire_get_longueur_mm(&trajet_trajectoire) - position_mm;
-    distance_fin_trajectoire_mm=distance_contrainte;
-    // En cas de dépassement, on veut garder la contrainte, pour l'instant
-    if(distance_contrainte > 0){
-        vitesse_max_contrainte = sqrtf(2 * acceleration_mm_ss * distance_contrainte);
-    }else{
-        vitesse_max_contrainte = 0;
-    }
-
-    distance_contrainte_obstacle = Trajet_get_obstacle_mm();
-    if(distance_contrainte_obstacle != DISTANCE_INVALIDE){
-        vitesse_max_contrainte_obstacle = sqrtf(2 * acceleration_mm_ss_obstacle * distance_contrainte_obstacle);
-        if(vitesse_max_contrainte_obstacle < vitesse_max_contrainte){
-            vitesse_max_contrainte = vitesse_max_contrainte_obstacle;
-        }
-    }/*
-    if((Trajet_get_obstacle_mm() != DISTANCE_INVALIDE) && (Trajet_get_obstacle_mm() < 50)){
-        vitesse = 0;
-    }*/
-    
-
-    // Selection de la vitesse la plus faible
-    if(vitesse > vitesse_max_contrainte){
-        vitesse = vitesse_max_contrainte;
-    }
-    if(vitesse > vitesse_max_trajet_mm_s){
-        vitesse = vitesse_max_trajet_mm_s;
-    }
-    return vitesse;
-}
-
-
-float Trajet_get_obstacle_mm(void){
-    return distance_obstacle_mm;
-}
-
-void Trajet_set_obstacle_mm(float distance_mm){
-    distance_obstacle_mm = distance_mm;
-}
-
-
-/// @brief Renvoi l'angle d'avancement du robot dans le référentiel du terrain
-/// @return angle en radian.
-float Trajet_get_orientation_avance(){
-    struct point_xyo_t point, point_suivant;
-    float avance_abscisse = 0.01;
-    float angle;
-
-    if(abscisse >= 1){
-        return 0;
-    }
-    if(abscisse + avance_abscisse >= 1){
-        avance_abscisse = 1 - abscisse;
-    }
-
-    point = Trajectoire_get_point(&trajet_trajectoire, abscisse);
-    point_suivant = Trajectoire_get_point(&trajet_trajectoire, abscisse + avance_abscisse);
-
-    angle = atan2f(point_suivant.point_xy.y - point.point_xy.y, point_suivant.point_xy.x - point.point_xy.x);
-    return angle;
-}
-
-void Trajet_inverse(){
-    float old_abscisse = abscisse;
-    float old_position_mm = position_mm;
-    Trajectoire_inverse(&trajet_trajectoire);
-    Trajet_debut_trajectoire(trajet_trajectoire);
-    abscisse = 1 - old_abscisse;
-    position_mm = Trajectoire_get_longueur_mm(&trajet_trajectoire) - old_position_mm;
-}
-
-float Trajet_get_abscisse(){
-    return abscisse;
-}
-
-/// @brief Indique si le robot est bloqué sur le trajet
-/// @return 0 si le robot n'est pas bloqué, 1 s'il est bloqué
-uint32_t Trajet_get_bloque(){
-    if(Trajet_get_obstacle_mm() == DISTANCE_INVALIDE){
-        return 0;
-    }
-    if (vitesse_max_contrainte_obstacle == 0){
-        return 1;
-    }
-    return 0;
-}

Trajet.h

@@ -1,37 +0,0 @@
-#include "pico/stdlib.h"
-#include "Trajectoire.h"
-
-#ifndef TRAJET_H
-#define TRAJET_H
-
-enum etat_trajet_t{
-    TRAJET_EN_COURS,
-    TRAJET_TERMINE
-};
-
-// Vitesse et acceleration pour translation pure (en mm/s et mm/s²)
-#define TRAJECT_CONFIG_RAPIDE 300, 1200
-#define TRAJECT_CONFIG_RAPIDE_ROUGE 500, 1200
-// Vitesse et acceleration pour un mouvement complexe (en mm et mm/s²)
-#define TRAJECT_CONFIG_AVANCE_ET_TOURNE 300, 500
-// Vitesse et acceleration - standard (en mm et mm/s²)
-#define TRAJECT_CONFIG_STD 300, 300
-// Vitesse et acceleration pour une rotation (rad/s et rad/s²)
-#define TRAJECT_CONFIG_ROTATION_PURE 2, 2
-
-extern const float distance_pas_obstacle;
-
-void Trajet_init(int);
-void Trajet_config(float _vitesse_max_trajet_mm_s, float _acceleration_mm_ss);
-void Trajet_debut_trajectoire(struct trajectoire_t trajectoire);
-enum etat_trajet_t Trajet_avance(float temps_s);
-struct position_t Trajet_get_consigne(void);
-float Trajet_get_obstacle_mm(void);
-void Trajet_set_obstacle_mm(float distance_mm);
-void Trajet_stop(float);
-float Trajet_get_orientation_avance(void);
-float Trajet_get_abscisse();
-uint32_t Trajet_get_bloque();
-void Trajet_inverse();
-
-#endif

VL53L8CX_ULD_API/src/vl53l8cx_api.c

@@ -11,6 +11,7 @@
   */
 
 #include <stdlib.h>
+#include <stdio.h>
 #include <string.h>
 #include "vl53l8cx_api.h"
 #include "vl53l8cx_buffers.h"
@@ -50,6 +51,7 @@ static uint8_t _vl53l8cx_poll_for_answer(
 		{
 			timeout++;
 		}
+		//printf("expected %d / %d\n", expected_value, (p_dev->temp_buffer[pos] & mask ));
 	}while ((p_dev->temp_buffer[pos] & mask) != expected_value);
 
 	return status;
@@ -236,6 +238,7 @@ uint8_t vl53l8cx_is_alive(
 	if((device_id == (uint8_t)0xF0) && (revision_id == (uint8_t)0x0C))
 	{
 		*p_is_alive = 1;
+		//printf("device_id:%d,revision_id:%d\n",device_id, revision_id);
 	}
 	else
 	{
@@ -259,6 +262,7 @@ uint8_t vl53l8cx_init(
 	p_dev->is_auto_stop_enabled = (uint8_t)0x0;
 
 	/* SW reboot sequence */
+	printf("Rebbot sequence\n");
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
 	status |= WrByte(&(p_dev->platform), 0x0009, 0x04);
 	status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
@@ -292,6 +296,7 @@ uint8_t vl53l8cx_init(
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);
 
 	/* Enable FW access */
+	printf("Enable FW access\n");
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
 	status |= WrByte(&(p_dev->platform), 0x06, 0x01);
 	status |= _vl53l8cx_poll_for_answer(p_dev, 1, 0, 0x21, 0xFF, 0x4);
@@ -327,6 +332,7 @@ uint8_t vl53l8cx_init(
 	status |= WrByte(&(p_dev->platform), 0x20, 0x06);
 
 	/* Download FW into VL53L8CX */
+	printf("Download FW\n");
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x09);
 	status |= WrMulti(&(p_dev->platform),0,
 		(uint8_t*)&VL53L8CX_FIRMWARE[0],0x8000);
@@ -339,6 +345,7 @@ uint8_t vl53l8cx_init(
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
 
 	/* Check if FW correctly downloaded */
+	printf("Check FW\n");
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
 	status |= WrByte(&(p_dev->platform), 0x06, 0x03);
 
@@ -366,6 +373,7 @@ uint8_t vl53l8cx_init(
 	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);
 
 	/* Firmware checksum */
+	printf("Check checksum\n");
 	status |= RdMulti(&(p_dev->platform), (uint16_t)(0x812FFC & 0xFFFF),
 			p_dev->temp_buffer, 4);
 	SwapBuffer(p_dev->temp_buffer, 4);
@@ -376,6 +384,8 @@ uint8_t vl53l8cx_init(
 	}
 	//crc_checksum = *((uint32_t *)&p_dev->temp_buffer[0]);
 
+	printf("More stuff\n");
+
 	/* Get offset NVM data and store them into the offset buffer */
 	status |= WrMulti(&(p_dev->platform), 0x2fd8,
 		(uint8_t*)VL53L8CX_GET_NVM_CMD, sizeof(VL53L8CX_GET_NVM_CMD));

VL53L8_2024.c

@@ -1,6 +1,13 @@
 #include "vl53l8cx_api.h"
-#include "Trajet.h"
 #include <stdio.h>
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI 3.314159
+#endif
+
+#define DEGREE_EN_RADIAN (M_PI / 180.)
+#define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN)
 
 int masque[64]={
 
@@ -9,9 +16,21 @@ int masque[64]={
 	3000,3000,3000,3000,3000,3000,3000,3000,
 	3000,3000,3000,3000,3000,3000,3000,3000,
 	3000,3000,3000,3000,3000,3000,3000,3000,
-	350,350,350,350,350,350,350,350,
+	3000,3000,3000,3000,3000,3000,3000,3000,
-	281,286,299,306,310,314,302,286,
+	3000,3000,3000,3000,3000,3000,3000,3000,
-	195,200,202,206,213,200,200,202
+	3000,3000,3000,3000,3000,3000,3000,3000,
+};
+
+
+float angles_VL53L8[]={
+		7./16. * PLAGE_ANGLE_DEGREE,
+		5./16. * PLAGE_ANGLE_DEGREE, 
+		3./16. * PLAGE_ANGLE_DEGREE, 
+		1./16. * PLAGE_ANGLE_DEGREE, 
+		-1./16. * PLAGE_ANGLE_DEGREE, 
+		-3./16. * PLAGE_ANGLE_DEGREE, 
+		-5./16. * PLAGE_ANGLE_DEGREE, 
+		-7./16. * PLAGE_ANGLE_DEGREE
 	};
 
 float old_min_distance;
@@ -101,13 +120,15 @@ void VL53L8_init(VL53L8CX_Configuration * Dev){
 
 }
 
-
+/// @brief Lit le capteur, filtre les données avec les status renvoyés et le masque de distance défini plus haut
-void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){
+/// @param Dev Capteur à lire
-	uint8_t error;
+/// @param Results Stockage des résultats
+/// @return 0 si tout s'est bien passé, une erreur sinon
+uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){
+	uint8_t error=0;
     error = vl53l8cx_get_ranging_data(Dev, Results);
 	if(error){
-		printf("Error VL53L8\n");
+		return error;
-		VL53L8_init(Dev);
 	}
 
 
@@ -118,12 +139,6 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
     // Filtrage des données
     for(int row=0; row < 8; row++){
         for(int col=0; col < 8; col++){
-            /*if(Results.target_status[col+ 8*row] == 255 ||// No Target detected
-                Results.target_status[col+ 8*row] == 10 ||// Range valid, but no target detected at previous range
-                Results.target_status[col+ 8*row] == 4 ||// Target consistency failed
-                Results.target_status[col+ 8*row] == 3  ){// Sigma error too high
-                Results.distance_mm[col+ 8*row] = 3000;
-            }*/
             // Seul status 100% valid
             if(Results->target_status[col+ 8*row] != 5){
                 Results->distance_mm[col+ 8*row] = 3000;
@@ -136,17 +151,31 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
             }
         }
     }
+	return error;	
+}
 
-    // Affichage des données
+/// @brief Contrôle si des données sont disponibles, si oui, lit le données
-	/*
+/// @param capteur capteur à lire
-    printf(">distance:");
+/// @param results stockage des résultats
-    for(int row=0; row < 8; row++){
+/// @return 1 si des données ont été lues
-        for(int col=0; col < 8; col++){
+uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){
-            printf("%d,", Results->distance_mm[col+ 8*row]);
+  uint8_t isReady, status;
+
+  status = vl53l8cx_check_data_ready(capteur, &isReady);
+  if(status){
+		printf("erreur:%d\n", status);
 	}
+  if(isReady)
+  {
+    //printf(">r%d:1\n",capteur->platform.address);
+    status = VL53L8_lecture(capteur, results);
+    if(status){
+      printf("erreur:%d\n", status);
     }
-    printf("\n");
+  }else{
-	*/
+    //Serial.printf(">r:0\n");
+  }
+  return isReady;
 }
 
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
@@ -165,8 +194,152 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 		*distance = min_distance-50;
 	}
 	old_min_distance = *distance;
+}
 
+const int selected_row = 5;
 
+/// @brief Renvoie la position de l'extrémité gauche de la planche
+/// @param Results Valeurs brutes du capteurs
+/// @param pos_x position de la planche 
+/// @return 0 sucess, 1 failed, 2 failed: no detection, 3 failed: full detection
+int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
+	uint16_t mesure1, mesure2;
+	int min_distance = 2000;
+	int col, row;
+	int distance_ref_mm = 500;
+	int last_col_avec_planche=0;
+	int full_detection = 1;
+	int no_detection = 1;
+
+	*angle = NAN;
+	// Contrôle si la planche est à droite
+	for(col=7; col>=0; col--){
+		printf("%d, ", Results.distance_mm[col + 8*3]);
+		for(row=selected_row; row<=selected_row; row++){
+			// SI c'est la première mesure, on la prend comme référence
+			if(Results.distance_mm[col + 8*row] >  500){
+				// Au moins une mesure loin
+				full_detection = 0;
+			}
+			
+			if(distance_ref_mm >=  500){
+				distance_ref_mm = Results.distance_mm[col + 8*row];
+			}else{
+				// Au moins une mesure proche
+				no_detection = 0;
+				// Si on est plus proche que la distance de référence, on la prend comme référence.
+				if(distance_ref_mm > Results.distance_mm[col + 8*row]){
+					distance_ref_mm = Results.distance_mm[col + 8*row];
+				}
+				// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
+				if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
+					last_col_avec_planche = col+2;
+					// Double break;
+					col = 0;
+					break;
+				}
+			}
+		}
+	}
+	printf("\n");
+	row=selected_row;
+	
+	if(last_col_avec_planche == 0 || last_col_avec_planche > 6){
+		// Echec
+		*pos_x = 0;
+		if (no_detection){
+			return 2;
+		}
+		if(full_detection){
+			return 3;
+		}
+		return 1;
+	}
+
+	if(last_col_avec_planche < 7){
+		/// On peut déterminer un angle de la planche
+		float angle_planche;
+		angle_planche = atan2f((float) (Results.distance_mm[7 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), 
+			-(Results.distance_mm[7 + 8*row] * sinf(angles_VL53L8[7]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche])));
+		*angle = angle_planche;
+	}
+	*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
+	*pos_y = (float)distance_ref_mm;
+
+	return 0;
+}
+
+/// @brief Renvoie la position de l'extrémité droite de la planche
+/// @param Results Valeurs brutes du capteurs
+/// @param pos_x position de la planche 
+/// @return 0 sucess, 1 failed
+int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
+	uint16_t mesure1, mesure2;
+	int min_distance = 2000;
+	int col, row;
+	int distance_ref_mm = 500;
+	int last_col_avec_planche=0;
+	int full_detection = 1;
+	int no_detection = 1;
+
+	*angle = NAN;
+	// Controle si la planche est à gauche
+	for(col=0; col<8; col++){
+		printf("%d,", Results.distance_mm[col + 8*3]);
+		for(row=selected_row; row<=selected_row; row++){
+			
+			if(Results.distance_mm[col + 8*row] >  500){
+				// Au moins une mesure loin
+				full_detection = 0;
+			}
+
+			// SI c'est la première mesure, on la prend comme référence
+			if(distance_ref_mm >=  500){
+				distance_ref_mm = Results.distance_mm[col + 8*row];
+			}else{
+				// Au moins une mesure proche
+				no_detection = 0;
+				// Si on est plus proche que la distance de référence, on la prend comme référence.
+				if(distance_ref_mm > Results.distance_mm[col + 8*row]){
+					distance_ref_mm = Results.distance_mm[col + 8*row];
+				}
+				// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
+				if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
+					last_col_avec_planche = col-2;
+					// Double break;
+					col = 9;
+					break;
+				}
+			}
+		}
+	}
+	printf("\n");
+	row=selected_row;
+	
+	if(last_col_avec_planche == 0 || last_col_avec_planche > 7){
+		// Echec
+		*pos_x = 0;
+		if (no_detection){
+			return 2;
+		}
+		if(full_detection){
+			return 3;
+		}
+		return 1;
+	}
+
+	if(last_col_avec_planche > 0){
+		/// On peut déterminer un angle de la planche
+		float angle_planche;
+		angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), 
+			Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche]));
+		*angle = angle_planche;
+	}
+
+	*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
+	*pos_y = (float)distance_ref_mm;
+
+	return 0;
 }
 
 float VL53L8_get_old_min_distance(){

VL53L8_2024.h

@@ -1,7 +1,10 @@
 #include "vl53l8cx_api.h"
 
 void VL53L8_init(VL53L8CX_Configuration * Dev);
-void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
+uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
+uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results);
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
+int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle);
+int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle);
 
 float VL53L8_get_old_min_distance(void);

i2c_maitre.c

@@ -1,274 +0,0 @@
-#include "i2c_maitre.h"
-#include "hardware/gpio.h"
-#include "hardware/i2c.h"
-#include "pico/stdlib.h"
-#include <stdio.h>
-
-#define I2C1_SDA_PIN 14
-#define I2C1_SCL_PIN 15
-
-#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, 400 * 1000);
-
-    printf("Initialisation des broches\n");
-    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 0: en cours, 
-int i2c_lire_registre(char adresse_7_bits, char registre, 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 - adresse %x\n", adresse_7_bits);
-        return I2C_ECHEC;
-    }
-
-    statu = i2c_read_blocking (i2c1, adresse_7_bits, reception, len, 0);
-    if(statu == PICO_ERROR_GENERIC){
-        printf("I2C - lecture registre Echec - adresse %x\n", adresse_7_bits);
-        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

@@ -1,15 +0,0 @@
-#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, 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,65 @@
+/*
+ * 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>
+
+#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

@@ -6,21 +6,15 @@
 #include "pico/stdlib.h"
 #include "pico/multicore.h"
 #include "hardware/adc.h"
-#include "hardware/pwm.h"
+#include "hardware/spi.h"
-#include "Asser_Position.h"
+#include "hardware/i2c.h"
-#include "Asser_Moteurs.h"
+#include "i2c_slave.h"
-#include "Commande_vitesse.h"
-#include "i2c_maitre.h"
-#include "Localisation.h"
-#include "Moteurs.h"
 #include "Temps.h"
-#include "Trajectoire.h"
-#include "Trajet.h"
 #include "VL53L8_2024.h"
 #include "vl53l8cx_api.h"
+#include "ws2812.h"
 #include <stdio.h>
 #include <math.h>
-#include "QEI.h"
 
 #define LED1PIN 20
 #define TIRETTE_PIN 6
@@ -29,13 +23,29 @@
 #define COULEUR_BLEU 1
 #define COULEUR_JAUNE 0
 
+#define OFFSET_CAPTEUR_GAUCHE_X_MM (-170)
+#define OFFSET_CAPTEUR_DROIT_X_MM  (170)
+
+// XIAO RP2040
+#define SCK 2
+#define MISO 4
+#define MOSI 3
+#define D0 26
+#define D1 27
+#define D2 28
+#define D3 29
+#define SDA 6
+#define SCL 7
+
+#define I2C_SLAVE_SDA_PIN SDA
+#define I2C_SLAVE_SCL_PIN SCL
+#define I2C_SLAVE_ADDRESS 0x19
+
 void affichage(void);
 void gestion_affichage(void);
 void tension_batterie_init(void);
 uint16_t tension_batterie_lire(void);
 
-void identifiant_init(void);
-uint identifiant_lire(void);
 
 int get_tirette(int);
 int get_couleur(void);
@@ -50,28 +60,91 @@ float distance1_mm=0, distance2_mm=0;
 extern float abscisse; 
 extern struct point_xyo_t point;
 float vitesse;
-VL53L8CX_Configuration Dev;
+VL53L8CX_Configuration gauche, droit;
+
+uint8_t capteur_init(VL53L8CX_Configuration * capteur);
+
+
+// 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_raw(i2c);
+            context.mem_address_written = true;
+        } else {
+            // save into memory
+            context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
+            context.mem_address++;
+        }
+        break;
+    case I2C_SLAVE_REQUEST: // master is requesting data
+        // load from memory
+        i2c_write_byte_raw(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_envoi_32bits(int32_t value, char adresse){
+  context.mem[adresse] = value >> 24;
+  context.mem[adresse+1] = (value >> 16) & 0xFF;
+  context.mem[adresse+2] = (value >> 8) & 0xFF;
+  context.mem[adresse+3] = value & 0xFF;
+}
+
+static void i2c_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(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
+}
 
 void main(void)
 {
 	int ledpower = 500;
+	uint8_t tampon[10];
+	uint8_t temp, status;
 	VL53L8CX_ResultsData Results;
+	bool fin_match = false;
 
 
 
     stdio_init_all();
+	stdio_set_translate_crlf(&stdio_usb, false);
 	Temps_init();
 	//tension_batterie_init();
-	identifiant_init();
+	spi_init(spi0, 2000000);
-	Localisation_init(identifiant_lire());
+
-	Trajet_init(identifiant_lire());
+	ws2812_init();
-	i2c_maitre_init();
 
 	
 
 	uint32_t temps_ms = Temps_get_temps_ms();
 	uint32_t temps_depart_ms;
-	struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
 	float vitesse_mm_s=100;
 	
 
@@ -79,76 +152,215 @@ void main(void)
 	gpio_set_dir(LED1PIN, GPIO_OUT );
 	gpio_put(LED1PIN, 1);
 
+	// Initialisation de la liaison SPI0
+	gpio_set_function(2, GPIO_FUNC_SPI);
+	gpio_set_function(3, GPIO_FUNC_SPI);
+	gpio_set_function(4, GPIO_FUNC_SPI);
+
+	// Initialisation de la liaison SPI1
+	gpio_set_function(26, GPIO_FUNC_SPI);
+	gpio_set_function(27, GPIO_FUNC_SPI);
+	gpio_set_function(28, GPIO_FUNC_SPI);
+
+	gpio_set_function(16, GPIO_FUNC_NULL);
+	gpio_set_function(17, GPIO_FUNC_NULL);
+	gpio_set_function(18, GPIO_FUNC_NULL);
+	gpio_set_function(19, GPIO_FUNC_NULL);
+
+	gpio_init(1); // CS
+	gpio_set_dir(1, GPIO_OUT );
+	gpio_put(1, 1);
+
+	gpio_init(29); // CS
+	gpio_set_dir(29, GPIO_OUT );
+	gpio_put(29, 1);
+
+
+	gauche.platform.address = 1;
+	droit.platform.address = 29;
+	
+	spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+	spi_init(spi0, 2000000);
+
+	spi_set_format(spi1, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+	spi_init(spi1, 2000000);
+
+	tampon[0] = 0x55;
+	tampon[1] = 0x55;
+
+	i2c_setup_slave();
 
-	//multicore_launch_core1(gestion_affichage);
-	multicore_launch_core1(gestion_VL53L8CX);
 	sleep_ms(5000);
 	printf("Demarrage...\n");
 	
+	//multicore_launch_core1(gestion_affichage);
 	
 	
-	configure_trajet(identifiant_lire(), get_couleur());
+	gestion_VL53L8CX();
 	
 	
-	float vitesse_init =300;
-	vitesse = vitesse_init;
 
-	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
-
-	while(get_tirette(identifiant_lire()));
-	gpio_put(LED1PIN, 0);
-
-	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
-	if(identifiant_lire() == 3){
-		sleep_ms(90000);
 }
 
-	temps_depart_ms = Temps_get_temps_ms();
 
+uint8_t capteur_init(VL53L8CX_Configuration * capteur){
+  uint8_t status=0;
+  printf("debut init\n");
+  //pinMode(capteur->platform.address, OUTPUT);
+
+  status = vl53l8cx_init(capteur);
+  if(status != 0){
     while(1){
+      printf("Status init = %d\n", status);
+      WaitMs(&(capteur->platform), 1000);
+      break;
+    }
+  }
+  sleep_ms(100);
 
-		// Fin du match 
+  status = vl53l8cx_set_resolution(capteur, VL53L8CX_RESOLUTION_8X8);
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
+	if(status !=0){
-			Moteur_Stop();
+		while(1){
-			while(1);
+			printf("vl53l8cx_set_resolution failed :%d\n", status);
+			WaitMs(&(capteur->platform), 1000);
+      break;
+		}
 	}
-		if(temps_ms != Temps_get_temps_ms()){
-			temps_ms = Temps_get_temps_ms();
-			if(temps_ms % step_ms == 0){
-				QEI_update();
-				Localisation_gestion();
 
-				if(etat_trajet != TRAJET_TERMINE){
+	status = vl53l8cx_set_ranging_frequency_hz(capteur, 15);
-					etat_trajet = Trajet_avance((float)step_ms/1000.);
+	if(status !=0){
-				}else{
+		while(1){
-					Asser_Position_maintien();
+			printf("vl53l8cx_set_ranging_frequency_hz  (15hz) failed :%d\n", status);
+			WaitMs(&(capteur->platform), 1000);
+      break;
 		}
-				AsserMoteur_Gestion(step_ms);
 	}
+
+	//vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST);
+	vl53l8cx_set_target_order(capteur, VL53L8CX_TARGET_ORDER_STRONGEST);
+
+  status = vl53l8cx_start_ranging(capteur);
+  
+  printf("Capteur : %d, fin init: %d\n", capteur->platform.address, status);
+  return status;
+
 }
 
 
 
+void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){
+  uint8_t row, col;
+  for(col=0; col<8; col++){
+    for(row=0; row<8; row++){
+      printf("%4d ", results_gauche->distance_mm[col*8 + row]);
     }
+    printf("  -  ");
+    for(row=0; row<8; row++){
+      printf("%4d ", results_droit->distance_mm[col*8 + row]);
+    }
+
+    printf("\n");
+  }
+  printf("\n");
 }
 
 /// @brief Obtient la distance de l'obstacle le plus proche.
 /// @param  
+
+float gauche_planche_pos_x, gauche_planche_pos_y, gauche_planche_angle;
+float droit_planche_pos_x, droit_planche_pos_y, droit_planche_angle;
+
+float planche_centre_x, planche_centre_y, planche_angle_rad;
+float taille_planche;
+int echec;
+
 void gestion_VL53L8CX(void){
-	VL53L8CX_ResultsData Results;
+	VL53L8CX_ResultsData results_gauche, results_droit;
+	int echec_gauche, echec_droit;
 	float distance_obstacle;
-	VL53L8_init(&Dev);
+	capteur_init(&gauche);
+	capteur_init(&droit);
 	sleep_ms(100);
-    VL53L8_lecture( &Dev, &Results); // une première lecture 
+    capteur_actualise( &gauche, &results_gauche); // une première lecture 
+	capteur_actualise( &droit, &results_droit); // une première lecture 
     uint8_t status, isReady;
 	while(1){
-		status = vl53l8cx_check_data_ready(&Dev, &isReady);
+		isReady = 0;
+		isReady |= capteur_actualise( &gauche, &results_gauche);
+		isReady |= capteur_actualise( &droit, &results_droit);
 		if(isReady){
-			VL53L8_lecture( &Dev, &Results);
+			capteurs_affiche_donnees(&results_gauche, &results_droit);
-			VL53L8_min_distance(Results, &distance_obstacle);
+			//VL53L8_lecture( &gauche, &Results);
-			Trajet_set_obstacle_mm(distance_obstacle);
+			echec = 0;
+			echec_gauche = VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y, &gauche_planche_angle);
+			echec_droit = VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y, &droit_planche_angle);
+			printf("g:%d,d:%d\n", echec_gauche, echec_droit );
+
+			droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_X_MM;
+			gauche_planche_pos_x = -gauche_planche_pos_x + OFFSET_CAPTEUR_GAUCHE_X_MM;
+
+			echec = (echec_gauche > 0) + (echec_droit > 0);
+
+			if(echec == 2){
+				if(echec_gauche == 2 && echec_droit == 3){
+					ws2812_set(0, 0, 0xF);
+					context.mem[0] = 3;
+				}else if(echec_gauche == 3 && echec_droit == 2){
+					ws2812_set(0, 0, 0xF);
+					context.mem[0] = 4;
+				}else{
+					// Aucun capteur valide
+					ws2812_set(0x0F,0,0);
+					context.mem[0] = 0;
+				}
+			}else if(echec == 1){
+				// Un seul capteur valide
+				if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){
+					// capteur gauche permet de déterminer la position de la planche
+					ws2812_set(0x0F,0x8,0);
+					planche_centre_x = gauche_planche_pos_x + 200 * cos(gauche_planche_angle);
+					planche_centre_y = gauche_planche_pos_y + 200 * sin(gauche_planche_angle);
+					planche_angle_rad = gauche_planche_angle;
+					i2c_envoi_32bits(planche_centre_x, 1);
+					i2c_envoi_32bits(planche_centre_y, 5);
+					i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
+					context.mem[0] = 1;
+					
+				}else if(echec_droit == 0 && ! isnan(droit_planche_angle)){
+					// capteur droit permet de déterminer la position de la planche
+					ws2812_set(0x0F,0x8,0);
+					planche_centre_x = droit_planche_pos_x - 200 * cos(droit_planche_angle);
+					planche_centre_y = droit_planche_pos_y - 200 * sin(droit_planche_angle);
+					planche_angle_rad = droit_planche_angle;
+					i2c_envoi_32bits(planche_centre_x, 1);
+					i2c_envoi_32bits(planche_centre_y, 5);
+					i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
+					context.mem[0] = 1;
+					
+				}else{
+					// On a un bout de la planche mais pas d'angle, c'est un echec
+					echec = 2;
+					ws2812_set(0x0F,0,0);
+					context.mem[0] = 0;
+				}
+				
+			}else{
+				// 2 capteurs valides
+				ws2812_set(0,0x0F,0);
+				planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2;
+				planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2;
+				planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x);
+				i2c_envoi_32bits(planche_centre_x, 1);
+				i2c_envoi_32bits(planche_centre_y, 5);
+				i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
+				context.mem[0] = 2;
+			}
+
+			
 		}
 		affichage();
+		printf("\n");
+		sleep_ms(150);
 	}
 }
 
@@ -162,174 +374,17 @@ void gestion_affichage(void){
 }
 
 void affichage(void){
-/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
-	printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
-	printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
-	printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
 	
-	printf(">abscisse:%f\n",abscisse);
+		printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x);
+		printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y);
+		printf(">planche_d_pos_x:%f\n",droit_planche_pos_x);
+		printf(">planche_d_pos_y:%f\n",droit_planche_pos_y);
+		printf(">planche_centre_x:%f\n",planche_centre_x);
+		printf(">planche_centre_y:%f\n",planche_centre_y);
+		printf(">planche_angle:%f\n",planche_angle_rad / M_PI * 180);
+		printf(">gauche_planche_angle:%f\n",gauche_planche_angle / M_PI * 180);
+		printf(">droit_planche_angle:%f\n",droit_planche_angle / M_PI * 180);
+		printf(">echec:%d\n", echec);
 		
-	struct position_t position_actuelle;
-	position_actuelle = Localisation_get();
-	printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
-	printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
-	printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
-	
-	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
-}
-
-void tension_batterie_init(void){
-	adc_init();
-    adc_gpio_init(28); // Analog_2
-    adc_select_input(2);
-    adc_run(1); // Free running mode
-}
-
-uint16_t tension_batterie_lire(){
-	uint16_t result = (uint16_t) adc_hw->result;
-	const float conversion_factor = 3.3f / (1 << 12);
-	float v_bat = result * conversion_factor * 11.;
-
-	return result;
-}
-
-void identifiant_init(){
-	gpio_init(21);
-	gpio_init(22);
-	gpio_init(26);
-	gpio_pull_up(21);
-	gpio_pull_up(22);
-	gpio_pull_up(26);
-	gpio_set_dir(21, GPIO_IN);
-	gpio_set_dir(22, GPIO_IN);
-	gpio_set_dir(26, GPIO_IN);
-
-	// Tirette
-	gpio_init(TIRETTE_PIN);
-	gpio_pull_up(TIRETTE_PIN);
-	gpio_set_dir(TIRETTE_PIN, GPIO_IN);
-
-	// Couleur
-	gpio_init(COULEUR_PIN);
-	gpio_pull_up(COULEUR_PIN);
-	gpio_set_dir(COULEUR_PIN, GPIO_IN);
-}
-
-int get_tirette(int id){
-	if(id == 3){
-		return !gpio_get(TIRETTE_PIN);
-	}
-	return (VL53L8_get_old_min_distance() <50);
-	
-}
-
-int get_couleur(void){
-	return gpio_get(COULEUR_PIN);
-}
-
-/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
-/// @return identifiant du robot (PDI switch)
-uint identifiant_lire(){
-	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
-}
-
-void configure_trajet(int identifiant, int couleur){
-	
-	struct trajectoire_t trajectoire;
-	Trajet_config(TRAJECT_CONFIG_RAPIDE);
-
-	switch(couleur){
-		case COULEUR_JAUNE:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 1:
-					Localisation_set(3000-1249, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
-					3000-750, 1400, 3000-750, 2100, 0, 0);
-					break;
-				case 2:
-					Localisation_set(3000-1245, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
-					3000-540, 1400, 3100, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(3000-1130, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
-					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(3000-1364, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
-					2700-900, 600, 2700-0, 0, 0, 0);
-					break;
-				case 5:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(3000-1450, 2000-63, 0);
-					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
-					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-
-		case COULEUR_BLEU:
-			switch (identifiant)
-			{
-				case 0:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 1:
-					Localisation_set(1249, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
-					750, 1400, 750, 2100, M_PI, M_PI);
-					break;
-				case 2:
-					Localisation_set(1245, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
-					540, 1400, -100, 1400, M_PI, M_PI);
-					break;
-				case 3:
-					Localisation_set(1121, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
-					606, 2000-590, 225, 2000-225, M_PI, M_PI);
-					break;
-				case 4:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(1364, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
-					900, 600, 0, 0, -M_PI / 2., M_PI);
-					break;
-				case 5:
-					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
-					Localisation_set(1450, 2000-63, M_PI);
-					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
-					930, 970, 3000, 1200, -M_PI / 2., M_PI);
-					break;
-				case 6:
-					break;
-				case 7:
-					break;
-				
-				default:
-					break;
-			}
-			break;
-	}
-	
-	Trajet_debut_trajectoire(trajectoire);
 
 }

quadrature_encoder.pio

@@ -1,165 +0,0 @@
-;
-; Copyright (c) 2021 pmarques-dev @ github
-;
-; SPDX-License-Identifier: BSD-3-Clause
-;
-
-.program quadrature_encoder
-
-; this code must be loaded into address 0, but at 29 instructions, it probably
-; wouldn't be able to share space with other programs anyway
-.origin 0
-
-
-; the code works by running a loop that continuously shifts the 2 phase pins into
-; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
-; does the proper "do nothing" | "increment" | "decrement" action for that pin
-; state change (or no change)
-
-; ISR holds the last state of the 2 pins during most of the code. The Y register
-; keeps the current encoder count and is incremented / decremented according to
-; the steps sampled
-
-; writing any non zero value to the TX FIFO makes the state machine push the
-; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case
-; sampling loop takes 14 cycles, so this program is able to read step rates up
-; to sysclk / 14  (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec)
-
-
-; 00 state
-	JMP update	; read 00
-	JMP decrement	; read 01
-	JMP increment	; read 10
-	JMP update	; read 11
-
-; 01 state
-	JMP increment	; read 00
-	JMP update	; read 01
-	JMP update	; read 10
-	JMP decrement	; read 11
-
-; 10 state
-	JMP decrement	; read 00
-	JMP update	; read 01
-	JMP update	; read 10
-	JMP increment	; read 11
-
-; to reduce code size, the last 2 states are implemented in place and become the
-; target for the other jumps
-
-; 11 state
-	JMP update	; read 00
-	JMP increment	; read 01
-decrement:
-	; note: the target of this instruction must be the next address, so that
-	; the effect of the instruction does not depend on the value of Y. The
-	; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
-	; is just a pure "decrement Y" instruction, with no other side effects
-	JMP Y--, update	; read 10
-
-	; this is where the main loop starts
-.wrap_target
-update:
-	; we start by checking the TX FIFO to see if the main code is asking for
-	; the current count after the PULL noblock, OSR will have either 0 if
-	; there was nothing or the value that was there
-	SET X, 0
-	PULL noblock
-
-	; since there are not many free registers, and PULL is done into OSR, we
-	; have to do some juggling to avoid losing the state information and
-	; still place the values where we need them
-	MOV X, OSR
-	MOV OSR, ISR
-
-	; the main code did not ask for the count, so just go to "sample_pins"
-	JMP !X, sample_pins
-
-	; if it did ask for the count, then we push it
-	MOV ISR, Y	; we trash ISR, but we already have a copy in OSR
-	PUSH
-
-sample_pins:
-	; we shift into ISR the last state of the 2 input pins (now in OSR) and
-	; the new state of the 2 pins, thus producing the 4 bit target for the
-	; computed jump into the correct action for this state
-	MOV ISR, NULL
-	IN OSR, 2
-	IN PINS, 2
-	MOV PC, ISR
-
-	; the PIO does not have a increment instruction, so to do that we do a
-	; negate, decrement, negate sequence
-increment:
-	MOV X, !Y
-	JMP X--, increment_cont
-increment_cont:
-	MOV Y, !X
-.wrap	; the .wrap here avoids one jump instruction and saves a cycle too
-
-
-
-% c-sdk {
-
-#include "hardware/clocks.h"
-#include "hardware/gpio.h"
-
-// max_step_rate is used to lower the clock of the state machine to save power
-// if the application doesn't require a very high sampling rate. Passing zero
-// will set the clock to the maximum, which gives a max step rate of around
-// 8.9 Msteps/sec at 125MHz
-
-static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
-{
-	pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
-	gpio_pull_up(pin);
-	gpio_pull_up(pin + 1);
-
-	pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
-	sm_config_set_in_pins(&c, pin); // for WAIT, IN
-	sm_config_set_jmp_pin(&c, pin); // for JMP
-	// shift to left, autopull disabled
-	sm_config_set_in_shift(&c, false, false, 32);
-	// don't join FIFO's
-	sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
-
-	// passing "0" as the sample frequency,
-	if (max_step_rate == 0) {
-		sm_config_set_clkdiv(&c, 1.0);
-	} else {
-		// one state machine loop takes at most 14 cycles
-		float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate);
-		sm_config_set_clkdiv(&c, div);
-	}
-
-	pio_sm_init(pio, sm, offset, &c);
-	pio_sm_set_enabled(pio, sm, true);
-}
-
-
-// When requesting the current count we may have to wait a few cycles (average
-// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple
-// encoders, we may request them all in one go and then fetch them all, thus
-// avoiding doing the wait multiple times. If we are reading just one encoder,
-// we can use the "get_count" function to request and wait
-
-static inline void quadrature_encoder_request_count(PIO pio, uint sm)
-{
-	pio->txf[sm] = 1;
-}
-
-static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm)
-{
-	while (pio_sm_is_rx_fifo_empty(pio, sm))
-		tight_loop_contents();
-	return pio->rxf[sm];
-}
-
-static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
-{
-	quadrature_encoder_request_count(pio, sm);
-	return quadrature_encoder_fetch_count(pio, sm);
-}
-
-%}
-

ws2812.c

@@ -0,0 +1,137 @@
+#include "pico/stdlib.h"
+#include "hardware/pio.h"
+#include "ws2812.h"
+#include "ws2812.pio.h"
+
+#define WS2812_PIN 12
+#define WS2812_POWER_PIN 11
+#define IS_RGBW false
+
+
+uint32_t couleur[13];
+uint32_t buffer_couleur[12];
+
+void ws2812_set_buffer(uint32_t couleur, uint8_t index_led);
+
+void ws2812_init(){
+    /*couleur[0]=0x000200;
+    couleur[1]=0x010200;
+    couleur[2]=0x020200;
+    couleur[3]=0x020100;
+    couleur[4]=0x020000;
+    couleur[5]=0x020001;
+    couleur[6]=0x020002;
+    couleur[7]=0x010002;
+    couleur[8]=0x000002;
+    couleur[9]=0x000102;
+    couleur[10]=0x000202;
+    couleur[11]=0x000201;
+    couleur[12]=0x000200;*/
+
+    couleur[0]=0x002000;
+    couleur[1]=0x102000;
+    couleur[2]=0x202000;
+    couleur[3]=0x201000;
+    couleur[4]=0x200000;
+    couleur[5]=0x200010;
+    couleur[6]=0x200020;
+    couleur[7]=0x100020;
+    couleur[8]=0x000020;
+    couleur[9]=0x001020;
+    couleur[10]=0x002020;
+    couleur[11]=0x002010;
+    couleur[12]=0x002000;
+
+    /*couleur[0]=0x00FF00;
+    couleur[1]=0x80FF00;
+    couleur[2]=0xFFFF00;
+    couleur[3]=0xFF8000;
+    couleur[4]=0xFF0000;
+    couleur[5]=0xFF0080;
+    couleur[6]=0xFF00FF;
+    couleur[7]=0x8000FF;
+    couleur[8]=0x0000FF;
+    couleur[9]=0x0080FF;
+    couleur[10]=0x00FFFF;
+    couleur[11]=0x00FF80;
+    couleur[12]=0x00FF00;*/
+
+    // initialisation du PIO 
+    PIO pio = pio0;
+    int sm = 0;
+    uint offset = pio_add_program(pio, &ws2812_program);
+
+
+    gpio_init(WS2812_POWER_PIN);
+    gpio_set_dir(WS2812_POWER_PIN, 1); // OUT
+    gpio_put(WS2812_POWER_PIN, 1);
+
+
+    ws2812_program_init(pio, sm, offset, WS2812_PIN, 800000, IS_RGBW);
+
+    // Tout rouge !
+    for(uint32_t i = 0; i<12; i++){
+        ws2812_set_buffer_rgb(0x4, 0x1, 0, i);
+    }
+    ws2812_affiche_buffer();
+}
+
+void ws2812_set(int rouge, int vert, int bleu){
+    ws2812_set_buffer_rgb(rouge, vert, bleu, 0);
+    ws2812_affiche_buffer();
+}
+
+void ws2812_arc_en_ciel(){
+    while(1){
+        uint32_t i;
+        sleep_ms(50);
+
+        // Affichage
+        for(i = 0; i<12; i++){
+            ws2812_set_buffer(couleur[i], i);
+        }
+        ws2812_affiche_buffer();
+
+        // Décalage des couleurs
+        for(i = 0; i<12; i++){
+            couleur[i] = couleur[i+1];
+        }
+        couleur[12]=couleur[0];
+    }
+}
+
+static inline void put_pixel(uint32_t pixel_grb) {
+    pio_sm_put_blocking(pio0, 0, pixel_grb << 8u);
+}
+
+static inline uint32_t urgb_u32(uint8_t r, uint8_t g, uint8_t b) {
+    return
+            ((uint32_t) (r) << 8) |
+            ((uint32_t) (g) << 16) |
+            (uint32_t) (b);
+}
+
+void ws2812_affiche_buffer(){
+    for(uint32_t i = 0; i<12; i++){
+        put_pixel(buffer_couleur[i]);
+    }
+}
+
+void ws2812_set_buffer_rgb(uint8_t rouge, uint8_t vert, uint8_t bleu, uint8_t index_led){
+        ws2812_set_buffer(urgb_u32(rouge, vert,bleu), index_led);
+}
+
+//Bit    7  6  5  4  3  2  1  0
+//Data   R  R  R  G  G  G  B  B
+void ws2812_set_buffer_8bits(uint8_t couleur, uint8_t index_led){
+        ws2812_set_buffer(urgb_u32(couleur >> 5, (couleur >> 2) & 0x07, couleur & 0x03), index_led);
+}
+
+/// @brief Rempli le buffer à envoyer au LED, necessite d'appeler la fonction ws2812_affiche_buffer() ensuite
+/// @param couleur : couleur en RVB
+/// @param index_led : index entre 0 et 11
+void ws2812_set_buffer(uint32_t couleur, uint8_t index_led){
+    if(index_led <12){
+        buffer_couleur[index_led] = couleur;
+    }
+}

ws2812.h

@@ -0,0 +1,8 @@
+#include "pico/stdlib.h"
+
+void ws2812_init(void);
+void ws2812_set(int rouge, int vert, int bleu);
+void ws2812_affiche_buffer(void);
+void ws2812_set_buffer_rgb(uint8_t rouge, uint8_t vert, uint8_t bleu, uint8_t index_led);
+void ws2812_set_buffer_8bits(uint8_t couleur, uint8_t index_led);
+void ws2812_arc_en_ciel(void);

ws2812.pio

@@ -0,0 +1,85 @@
+;
+; Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+;
+; SPDX-License-Identifier: BSD-3-Clause
+;
+
+.program ws2812
+.side_set 1
+
+.define public T1 2
+.define public T2 5
+.define public T3 3
+
+.lang_opt python sideset_init = pico.PIO.OUT_HIGH
+.lang_opt python out_init     = pico.PIO.OUT_HIGH
+.lang_opt python out_shiftdir = 1
+
+.wrap_target
+bitloop:
+    out x, 1       side 0 [T3 - 1] ; Side-set still takes place when instruction stalls
+    jmp !x do_zero side 1 [T1 - 1] ; Branch on the bit we shifted out. Positive pulse
+do_one:
+    jmp  bitloop   side 1 [T2 - 1] ; Continue driving high, for a long pulse
+do_zero:
+    nop            side 0 [T2 - 1] ; Or drive low, for a short pulse
+.wrap
+
+% c-sdk {
+#include "hardware/clocks.h"
+
+static inline void ws2812_program_init(PIO pio, uint sm, uint offset, uint pin, float freq, bool rgbw) {
+
+    pio_gpio_init(pio, pin);
+    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
+
+    pio_sm_config c = ws2812_program_get_default_config(offset);
+    sm_config_set_sideset_pins(&c, pin);
+    sm_config_set_out_shift(&c, false, true, rgbw ? 32 : 24);
+    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
+
+    int cycles_per_bit = ws2812_T1 + ws2812_T2 + ws2812_T3;
+    float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
+    sm_config_set_clkdiv(&c, div);
+
+    pio_sm_init(pio, sm, offset, &c);
+    pio_sm_set_enabled(pio, sm, true);
+}
+%}
+
+.program ws2812_parallel
+
+.define public T1 2
+.define public T2 5
+.define public T3 3
+
+.wrap_target
+    out x, 32
+    mov pins, !null [T1-1]
+    mov pins, x     [T2-1]
+    mov pins, null  [T3-2]
+.wrap
+
+% c-sdk {
+#include "hardware/clocks.h"
+
+static inline void ws2812_parallel_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) {
+    for(uint i=pin_base; i<pin_base+pin_count; i++) {
+        pio_gpio_init(pio, i);
+    }
+    pio_sm_set_consecutive_pindirs(pio, sm, pin_base, pin_count, true);
+
+    pio_sm_config c = ws2812_parallel_program_get_default_config(offset);
+    sm_config_set_out_shift(&c, true, true, 32);
+    sm_config_set_out_pins(&c, pin_base, pin_count);
+    sm_config_set_set_pins(&c, pin_base, pin_count);
+    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
+
+    int cycles_per_bit = ws2812_parallel_T1 + ws2812_parallel_T2 + ws2812_parallel_T3;
+    float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
+    sm_config_set_clkdiv(&c, div);
+
+    pio_sm_init(pio, sm, offset, &c);
+    pio_sm_set_enabled(pio, sm, true);
+}
+%}