diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 05ca5f8..7560cfd 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -3,6 +3,7 @@
         "myDefaultIncludePath": [
             "${workspaceFolder}",
             "${workspaceFolder}/build",
+            "${myDefaultIncludePath}/Platform/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",
diff --git a/Asser_Moteurs.c b/Asser_Moteurs.c
deleted file mode 100644
index 0098144..0000000
--- a/Asser_Moteurs.c
+++ /dev/null
@@ -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);
-
-    }
-}
\ No newline at end of file
diff --git a/Asser_Moteurs.h b/Asser_Moteurs.h
deleted file mode 100644
index 42adbda..0000000
--- a/Asser_Moteurs.h
+++ /dev/null
@@ -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);
\ No newline at end of file
diff --git a/Asser_Position.c b/Asser_Position.c
deleted file mode 100644
index 74870ff..0000000
--- a/Asser_Position.c
+++ /dev/null
@@ -1,78 +0,0 @@
-#include "Localisation.h"
-#include "Commande_vitesse.h"
-#include "math.h"
-
-#define GAIN_P_POSITION 15
-#define GAIN_P_ORIENTATION 10
-
-#define MAX_ERREUR_ANGLE (30 * DEGRE_EN_RADIAN)
-
-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);
-}
-
-float Asser_Position_get_erreur_angle(){
-    return delta_orientation_radian;
-}
-
-/// @brief Renvoi 1 si l'erreur d'angle supérieur au seuil
-/// @return 1 si panic, 0 si nominal
-int Asser_Position_panic_angle(){
-    if(delta_orientation_radian > MAX_ERREUR_ANGLE){
-        return 1;
-    }
-    return 0;
-}
diff --git a/Asser_Position.h b/Asser_Position.h
deleted file mode 100644
index aa2e802..0000000
--- a/Asser_Position.h
+++ /dev/null
@@ -1,6 +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();
-
-int Asser_Position_panic_angle();
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9f23d3..cb6362c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,30 +2,23 @@ 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
-  Asser_Position.c
-  Asser_Moteurs.c
-  Commande_vitesse.c
+
+add_executable(VL53L8X_Gradin
   Geometrie.c
   i2c_maitre.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
@@ -34,25 +27,22 @@ add_executable(Mon_Projet
   Platform/platform.c
 )
 
-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(Mon_Projet
+target_link_libraries(VL53L8X_Gradin
   hardware_adc 
-  hardware_i2c 
-  hardware_pwm 
-  hardware_pio
+  hardware_spi
+  hardware_i2c
   pico_stdlib
   pico_multicore
 )
 
-pico_enable_stdio_usb(Mon_Projet 1)
-pico_enable_stdio_uart(Mon_Projet 1)
+pico_enable_stdio_usb(VL53L8X_Gradin 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
-    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/Mon_Projet.uf2
+    DEPENDS VL53L8X_Gradin
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/VL53L8X_Gradin.uf2
 )
diff --git a/Commande_vitesse.c b/Commande_vitesse.c
deleted file mode 100644
index c831fee..0000000
--- a/Commande_vitesse.c
+++ /dev/null
@@ -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);
-}
\ No newline at end of file
diff --git a/Commande_vitesse.h b/Commande_vitesse.h
deleted file mode 100644
index 609078f..0000000
--- a/Commande_vitesse.h
+++ /dev/null
@@ -1,2 +0,0 @@
-void commande_vitesse(float vitesse_avance_mm_s, float orientation_radian_s);
-void commande_vitesse_stop(void);
\ No newline at end of file
diff --git a/Localisation.c b/Localisation.c
deleted file mode 100644
index e2e735e..0000000
--- a/Localisation.c
+++ /dev/null
@@ -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;
-}
diff --git a/Localisation.h b/Localisation.h
deleted file mode 100644
index de101f6..0000000
--- a/Localisation.h
+++ /dev/null
@@ -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);
\ No newline at end of file
diff --git a/Moteurs.c b/Moteurs.c
deleted file mode 100644
index a3dc9a8..0000000
--- a/Moteurs.c
+++ /dev/null
@@ -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);
-}
\ No newline at end of file
diff --git a/Moteurs.h b/Moteurs.h
deleted file mode 100644
index 1eed7da..0000000
--- a/Moteurs.h
+++ /dev/null
@@ -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);
diff --git a/Platform/platform.c b/Platform/platform.c
index 209d1dc..ec356af 100644
--- a/Platform/platform.c
+++ b/Platform/platform.c
@@ -13,101 +13,56 @@
 
 #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 I2C_FAILED 1
-#define I2C_BUFFER_EXCEEDED 2
+#define BUFFER_SIZE 0x1000
+#define ERR_OK 0
 
-#define I2C_DEVICE i2c1
-
-#define MAX_I2C_BUFFER 0x8100
-
-
-/// @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_write_register(char adresse_7_bits, uint16_t index, uint8_t * values, uint32_t count){
-    int statu;
-	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.
-	timeout_time = time_us_64() + 5000000;
-    statu = i2c_write_blocking_until (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0, timeout_time);
-	//statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0);
-    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;
+void SPI_beginTransaction(char my_ss_pin){
+  /* switch(my_ss_pin){
+    case D0: SPI.begin(SCK, MISO, MOSI, SS); break;
+    case D1: SPI.begin(SCK, D3, MOSI, SS); break;
+  }*/
+  // 
+  gpio_put(my_ss_pin, 0);
+  
 }
 
-
-/// @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;
+void SPI_endTransaction(char my_ss_pin){
+  //SPI.endTransaction();
+  gpio_put(my_ss_pin, 1); 
+  //SPI.end();
 }
 
-
 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(spi0, tampon, 2);
+  spi_read_blocking(spi0, 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 +71,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(spi0, tampon, 3);
+   
+  SPI_endTransaction(my_ss_pin);
+  
+
+  return ERR_OK;
 
 }
 
@@ -128,9 +100,32 @@ uint8_t WrMulti(
 		uint8_t *p_values,
 		uint32_t size)
 {
-	uint8_t status = 255;
-	
-	return i2c_write_register(p_platform->address >> 1, RegisterAdress, p_values, size);
+  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;
+
+  SPI_beginTransaction(my_ss_pin);
+  
+  tampon[0] = RegisterAdress >> 8 | 0x80;
+  tampon[1] = RegisterAdress & 0xFF;
+  spi_write_blocking(spi0, tampon, 2);
+
+  while(size > BUFFER_SIZE){
+    memcpy(my_buffer, (uint8_t*)&p_values[index], BUFFER_SIZE);
+    spi_write_blocking(spi0, 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(spi0, my_buffer, size);
+  }
+  
+  SPI_endTransaction(my_ss_pin);
+
+  return ERR_OK;
 
 }
 
@@ -141,8 +136,36 @@ uint8_t RdMulti(
 		uint32_t size)
 {
 	uint8_t status = 255;
-	
-	return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_values, size);
+  uint32_t index =0;
+  uint32_t buffer_size = 32;
+  uint8_t my_ss_pin;
+  uint8_t tampon[2];
+  my_ss_pin = p_platform->address;
+
+  SPI_beginTransaction(my_ss_pin);
+  
+  tampon[0] = RegisterAdress >> 8 & 0x7F;
+  tampon[1] = RegisterAdress & 0xFF;
+
+  spi_write_blocking(spi0, tampon, 2);
+  spi_read_blocking(spi0, 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(spi0, 0, &(p_values[index]), buffer_size);
+    index += buffer_size;
+    size -= buffer_size;
+  }
+  if(size > 0){
+  	spi_read_blocking(spi0, 0, &(p_values[index]), size);
+  }
+  */
+  
+  SPI_endTransaction(my_ss_pin);
+  
+  return ERR_OK;
 	
 }
 
@@ -193,6 +216,6 @@ uint8_t WaitMs(
 {
 	/* Need to be implemented by customer. This function returns 0 if OK */
 	sleep_ms(TimeMs);
-	
+	//delay(TimeMs);
 	return 0;
-}
+}
\ No newline at end of file
diff --git a/QEI.c b/QEI.c
deleted file mode 100644
index c95dbe3..0000000
--- a/QEI.c
+++ /dev/null
@@ -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;
-}
\ No newline at end of file
diff --git a/QEI.h b/QEI.h
deleted file mode 100644
index e031360..0000000
--- a/QEI.h
+++ /dev/null
@@ -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);
\ No newline at end of file
diff --git a/Trajectoire.c b/Trajectoire.c
deleted file mode 100644
index c099490..0000000
--- a/Trajectoire.c
+++ /dev/null
@@ -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));
-    
-}
diff --git a/Trajectoire.h b/Trajectoire.h
deleted file mode 100644
index aecda2b..0000000
--- a/Trajectoire.h
+++ /dev/null
@@ -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
diff --git a/Trajectoire_bezier.c b/Trajectoire_bezier.c
deleted file mode 100644
index 5c4aeab..0000000
--- a/Trajectoire_bezier.c
+++ /dev/null
@@ -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;
-}
\ No newline at end of file
diff --git a/Trajectoire_bezier.h b/Trajectoire_bezier.h
deleted file mode 100644
index 5930684..0000000
--- a/Trajectoire_bezier.h
+++ /dev/null
@@ -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);
\ No newline at end of file
diff --git a/Trajectoire_circulaire.c b/Trajectoire_circulaire.c
deleted file mode 100644
index 2ec8726..0000000
--- a/Trajectoire_circulaire.c
+++ /dev/null
@@ -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;
-}
diff --git a/Trajectoire_circulaire.h b/Trajectoire_circulaire.h
deleted file mode 100644
index 6229d88..0000000
--- a/Trajectoire_circulaire.h
+++ /dev/null
@@ -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);
diff --git a/Trajectoire_droite.c b/Trajectoire_droite.c
deleted file mode 100644
index a2fa2a8..0000000
--- a/Trajectoire_droite.c
+++ /dev/null
@@ -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;
-}
\ No newline at end of file
diff --git a/Trajectoire_droite.h b/Trajectoire_droite.h
deleted file mode 100644
index e4ed24c..0000000
--- a/Trajectoire_droite.h
+++ /dev/null
@@ -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);
\ No newline at end of file
diff --git a/Trajet.c b/Trajet.c
deleted file mode 100644
index 7808b21..0000000
--- a/Trajet.c
+++ /dev/null
@@ -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;
-}
\ No newline at end of file
diff --git a/Trajet.h b/Trajet.h
deleted file mode 100644
index 84438ac..0000000
--- a/Trajet.h
+++ /dev/null
@@ -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
diff --git a/VL53L8CX_ULD_API/src/vl53l8cx_api.c b/VL53L8CX_ULD_API/src/vl53l8cx_api.c
index 88347b6..eda29fb 100644
--- a/VL53L8CX_ULD_API/src/vl53l8cx_api.c
+++ b/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));
diff --git a/VL53L8_2024.c b/VL53L8_2024.c
index cdde1f5..fb5c593 100644
--- a/VL53L8_2024.c
+++ b/VL53L8_2024.c
@@ -1,6 +1,9 @@
 #include "vl53l8cx_api.h"
-#include "Trajet.h"
 #include <stdio.h>
+#include <math.h>
+
+#define DEGREE_EN_RADIAN (M_PI / 180.)
+#define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN)
 
 int masque[64]={
 
@@ -9,11 +12,23 @@ 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,
-	281,286,299,306,310,314,302,286,
-	195,200,202,206,213,200,200,202
+	3000,3000,3000,3000,3000,3000,3000,3000,
+	3000,3000,3000,3000,3000,3000,3000,3000,
+	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;
 
 void VL53L8_init(VL53L8CX_Configuration * Dev){
@@ -145,8 +160,8 @@ void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results
             printf("%d,", Results->distance_mm[col+ 8*row]);
         }
     }
-    printf("\n");
-	*/
+    printf("\n");*/
+	
 }
 
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
@@ -165,8 +180,51 @@ int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
 		*distance = min_distance-50;
 	}
 	old_min_distance = *distance;
+}
+
+/// @brief Renvoie la position 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(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y){
+	uint16_t mesure1, mesure2;
+	int min_distance = 2000;
+	int col, row;
+	int distance_ref_mm = 200;
+	int last_col_avec_planche=0;
+	// Controle si la planche est à gauche
+	for(col=0; col<8; col++){
+		printf("%d,", Results.distance_mm[col + 8*3]);
+		for(row=3; row<=3; row++){
+			// 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{
+				// 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;
+					// Double break;
+					col = 9;
+					break;
+				}
+			}
+		}
+	}
+	printf("\n");
 	
-	
+	if(last_col_avec_planche == 0){
+		// Echec
+		*pos_x = 0;
+		return 1;
+	}
+	*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(){
diff --git a/VL53L8_2024.h b/VL53L8_2024.h
index 6bcaf0c..4f6a7df 100644
--- a/VL53L8_2024.h
+++ b/VL53L8_2024.h
@@ -3,5 +3,6 @@
 void VL53L8_init(VL53L8CX_Configuration * Dev);
 void VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results);
 int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance);
+int VL53L8_pos_planche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y);
 
 float VL53L8_get_old_min_distance(void);
\ No newline at end of file
diff --git a/main.c b/main.c
index cedc1c1..a07a851 100644
--- a/main.c
+++ b/main.c
@@ -6,21 +6,13 @@
 #include "pico/stdlib.h"
 #include "pico/multicore.h"
 #include "hardware/adc.h"
-#include "hardware/pwm.h"
-#include "Asser_Position.h"
-#include "Asser_Moteurs.h"
-#include "Commande_vitesse.h"
+#include "hardware/spi.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 <stdio.h>
 #include <math.h>
-#include "QEI.h"
 
 #define LED1PIN 20
 #define TIRETTE_PIN 6
@@ -29,13 +21,21 @@
 #define COULEUR_BLEU 1
 #define COULEUR_JAUNE 0
 
+// XIAO RP2040
+#define SCK 2
+#define MISO 4
+#define MOSI 3
+#define D0 26
+#define D1 27
+#define D2 28
+#define D3 29
+
+
 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);
@@ -52,27 +52,28 @@ extern struct point_xyo_t point;
 float vitesse;
 VL53L8CX_Configuration Dev;
 
+uint8_t capteur_init(VL53L8CX_Configuration * capteur);
+
 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();
-	Localisation_init(identifiant_lire());
-	Trajet_init(identifiant_lire());
-	i2c_maitre_init();
+	spi_init(spi0, 2000000);
 
 	
 
 	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;
 	
 
@@ -80,79 +81,141 @@ void main(void)
 	gpio_set_dir(LED1PIN, GPIO_OUT );
 	gpio_put(LED1PIN, 1);
 
-	
-	//multicore_launch_core1(gestion_affichage);
-	multicore_launch_core1(gestion_VL53L8CX);
+	// Initialisation de la liaison SPI
+	gpio_set_function(2, GPIO_FUNC_SPI);
+	gpio_set_function(3, GPIO_FUNC_SPI);
+	gpio_set_function(4, 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);
+
+	Dev.platform.address = 1;
+
+	spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+	spi_init(spi0, 2000000);
+
+	tampon[0] = 0x55;
+	tampon[1] = 0x55;
+
 	sleep_ms(5000);
 	printf("Demarrage...\n");
 	
+	//multicore_launch_core1(gestion_affichage);
+	
+	
+	gestion_VL53L8CX();
+	
 	
 
-	configure_trajet(identifiant_lire(), get_couleur());
+}
 
-	
-	float vitesse_init =300;
-	vitesse = vitesse_init;
 
-	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
+uint8_t capteur_init(VL53L8CX_Configuration * capteur){
+  uint8_t status=0;
+  printf("debut init\n");
+  //pinMode(capteur->platform.address, OUTPUT);
 
-	while(get_tirette(identifiant_lire()));
-	gpio_put(LED1PIN, 0);
+  status = vl53l8cx_init(capteur);
+  if(status != 0){
+    while(1){
+      printf("Status init = %d\n", status);
+      WaitMs(&(capteur->platform), 1000);
+      break;
+    }
+  }
+  sleep_ms(100);
 
-	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
-	if(identifiant_lire() == 3){
-		sleep_ms(90000);
+  status = vl53l8cx_set_resolution(capteur, VL53L8CX_RESOLUTION_8X8);
+	if(status !=0){
+		while(1){
+			printf("vl53l8cx_set_resolution failed :%d\n", status);
+			WaitMs(&(capteur->platform), 1000);
+      break;
+		}
 	}
 
-	temps_depart_ms = Temps_get_temps_ms();
-
-	while(1){
-		
-		// Fin du match 
-		if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
-			Moteur_Stop();
-			while(1);
+	status = vl53l8cx_set_ranging_frequency_hz(capteur, 15);
+	if(status !=0){
+		while(1){
+			printf("vl53l8cx_set_ranging_frequency_hz  (15hz) 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){
-					etat_trajet = Trajet_avance((float)step_ms/1000.);
-				}else{
-					Asser_Position_maintien();
-					if(Asser_Position_panic_angle()){
-						fin_match=1;
-					}
-				}
-				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;
+
+}
+
+uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){
+  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 = vl53l8cx_get_ranging_data(capteur, results);
+    if(status){
+      printf("erreur:%d\n", status);
+    }
+  }else{
+    //Serial.printf(">r:0\n");
+  }
+  return isReady;
+}
+
+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 planche_pos_x, planche_pos_y;
 void gestion_VL53L8CX(void){
 	VL53L8CX_ResultsData Results;
 	float distance_obstacle;
-	VL53L8_init(&Dev);
+	capteur_init(&Dev);
 	sleep_ms(100);
-    VL53L8_lecture( &Dev, &Results); // une première lecture 
+    capteur_actualise( &Dev, &Results); // une première lecture 
     uint8_t status, isReady;
 	while(1){
-		status = vl53l8cx_check_data_ready(&Dev, &isReady);
+		isReady = capteur_actualise( &Dev, &Results);
 		if(isReady){
-			VL53L8_lecture( &Dev, &Results);
+			capteurs_affiche_donnees(&Results, &Results);
+			//VL53L8_lecture( &Dev, &Results);
 			VL53L8_min_distance(Results, &distance_obstacle);
-			Trajet_set_obstacle_mm(distance_obstacle);
+			VL53L8_pos_planche(Results, &planche_pos_x, &planche_pos_y);
 		}
-		affichage();
+		//affichage();
+		sleep_ms(50);
 	}
 }
 
@@ -166,9 +229,11 @@ void gestion_affichage(void){
 }
 
 void affichage(void){
+		printf(">planche_pox_x:%f\n",planche_pos_x);
+		printf(">planche_pox_y:%f\n",planche_pos_y);
 /*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(">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);
@@ -179,161 +244,5 @@ void affichage(void){
 	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);
-
+	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));*/
 }
\ No newline at end of file
diff --git a/quadrature_encoder.pio b/quadrature_encoder.pio
deleted file mode 100644
index d245d4b..0000000
--- a/quadrature_encoder.pio
+++ /dev/null
@@ -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);
-}
-
-%}
-