diff --git a/APDS_9960.c b/APDS_9960.c
index 7db55d2..710b1aa 100644
--- a/APDS_9960.c
+++ b/APDS_9960.c
@@ -1,10 +1,128 @@
-int APDS9960_Init(){
+#include "i2c_maitre.h"
+#include "pico/stdlib.h"
+#include <stdio.h>
+
+#define APDS_ADRESSE 0x39
+
+
+inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu);
+
+void APDS9960_Init(){
+    char registre;
+    char valeur;
+
+    i2c_maitre_init();
+    // Désactivation du module pour le configurer
+    registre = 0x80;
+    valeur = 0x0;
+    while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
+
+
     // Registres à configurer 
+    // ATIME (0x81): temps d'acquisition 
+    // | Valeur | Nb cycles |  Temps  |
+    // |      0 |       255 |  712 ms |
+    // |    182 |        72 |  200 ms |
+    // |    255 |         1 | 2,78 ms |
+    registre = 0x81;
+    valeur = 220;
+    while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
+    // CONTROL<AGAIN> (0x8F<1:0>): Gain
+    // 0 : x1
+    // 1 : x4
+    // 2 : x16
+    // 3 : x64
+    registre = 0x8F;
+    valeur = 0x03;
+    while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
+
+    // Temps d'attente : Wen, WTIME, WLONG
 
 
     // ENABLE (PON) Address : 0x80 - Après les autres registres !!
     // RESERVED | Gesture En | Prox. Interrupt En | ALS Interrupt En | Wait En | Proximity Detect En |  ALS En | Power On |
     // Ob0000 0011 => 0x03
+    registre = 0x80;
+    valeur = 0x3;
+    while(i2c_ecrire_registre(APDS_ADRESSE, registre, valeur) != I2C_SUCCES);
+
+}
+
+int APDS9960_Lire(){
+    unsigned char reception[8];
+    uint16_t clear, rouge, vert, bleu;
+    i2c_lire_registre(APDS_ADRESSE, 0x94, reception, 8);
+    clear = (uint16_t) reception[0] + (uint16_t) reception[1] << 8;
+    rouge = (uint16_t) reception[2] + (uint16_t) reception[3] << 8;
+    vert = (uint16_t) reception[4] + (uint16_t) reception[5] << 8;
+    bleu = (uint16_t) reception[6] + (uint16_t) reception[7] << 8;
 
 
+    printf("clear:%u\n", clear/256);
+    printf("rouge:%u\n", rouge/256);
+    printf("vert:%u\n", vert/256);
+    printf("bleu:%u\n", bleu/256);
+
+
+    cc3_rgb2tls(rouge/256, vert/256, bleu/256);
+
+    return 1;
+
+}
+
+inline void cc3_rgb2tls (uint8_t rouge, uint8_t vert, uint8_t bleu)
+{
+  uint8_t hue, sat, val;
+  uint8_t rgb_min, rgb_max;
+  rgb_max = rouge;
+  rgb_min = rouge;
+  if (rouge > rgb_max)
+    rgb_max = rouge;
+  if (vert > rgb_max)
+    rgb_max = vert;
+  if (bleu > rgb_max)
+    rgb_max = bleu;
+  if (rouge < rgb_min)
+    rgb_min = rouge;
+  if (vert < rgb_min)
+    rgb_min = vert;
+  if (bleu < rgb_min)
+    rgb_min = bleu;
+
+// compute V
+  val = rgb_max;
+  if (val == 0) {
+    printf("val:0");
+    hue = sat = 0;
+    return;
+  }
+
+// compute S
+  sat = 255 * (rgb_max - rgb_min) / val;
+  if (sat == 0) {
+    printf("sat:0");
+    return;
+  }
+
+// compute H
+  if (rgb_max == rouge) {
+    hue =
+      0 + 43 * (int16_t)(vert -
+                bleu) / (rgb_max - rgb_min);
+  }
+  else if (rgb_max == vert) {
+    hue =
+      85 + 43 * (int16_t)(bleu -
+                 rouge) / (rgb_max - rgb_min);
+  }
+  else {                        /* rgb_max == blue */
+
+    hue =
+      171 + 43 * (int16_t)(rouge -
+                  vert) / (rgb_max - rgb_min);
+  }
+
+  printf("val:%u\n", val);
+  printf("sat:%u\n", sat);
+  printf("teinte:%u\n", hue*360/250);
 }
\ No newline at end of file
diff --git a/APDS_9960.h b/APDS_9960.h
index e69de29..b434c53 100644
--- a/APDS_9960.h
+++ b/APDS_9960.h
@@ -0,0 +1,2 @@
+void APDS9960_Init(void);
+int APDS9960_Lire(void);
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c8e366..f76e32d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ QEI.c
 gyro.c
 gyro_L3GD20H.c
 gyro_ADXRS453.c
+i2c_maitre.c
 Localisation.c
 Moteurs.c
 Robot_config.c
@@ -36,9 +37,9 @@ add_definitions(-DGYRO_ADXRS453)
 pico_enable_stdio_usb(test 1)
 pico_enable_stdio_uart(test 1)
 pico_add_extra_outputs(test)
-target_link_libraries(test pico_stdlib hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore)
+target_link_libraries(test pico_stdlib hardware_i2c hardware_timer hardware_spi hardware_pwm hardware_structs hardware_pio pico_multicore)
 
 add_custom_target(Flash
     DEPENDS test
-    COMMAND sudo picotool load ${PROJECT_BINARY_DIR}/test.uf2
+    COMMAND sudo picotool load -f ${PROJECT_BINARY_DIR}/test.uf2
 )
\ No newline at end of file
diff --git a/Holonome2023.c b/Holonome2023.c
new file mode 100644
index 0000000..c789d82
--- /dev/null
+++ b/Holonome2023.c
@@ -0,0 +1,129 @@
+#include <stdio.h>
+#include "pico/multicore.h"
+#include "pico/stdlib.h"
+#include "hardware/gpio.h"
+#include "hardware/timer.h"
+#include "pico/binary_info.h"
+#include "math.h"
+#include "Test.h"
+
+#include "gyro.h"
+#include "Asser_Moteurs.h"
+#include "Asser_Position.h"
+#include "Commande_vitesse.h"
+#include "Localisation.h"
+#include "Moteurs.h"
+#include "QEI.h"
+#include "Servomoteur.h"
+#include "spi_nb.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+
+const uint LED_PIN = 25;
+const uint LED_PIN_ROUGE = 28;
+const uint LED_PIN_NE_PAS_UTILISER = 22;
+
+uint temps_cycle;
+
+
+#define V_INIT -999.0
+#define TEST_TIMEOUT_US 10000000
+
+int mode_test();
+
+int main() {
+    bi_decl(bi_program_description("This is a test binary."));
+    bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
+    double vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
+    struct t_angle_gyro_double angle_gyro;
+
+    uint32_t temps_ms = 0, temps_ms_old;
+    uint32_t temps_us_debut_cycle;
+
+    stdio_init_all();
+
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, GPIO_OUT);
+    gpio_put(LED_PIN, 1);
+
+    gpio_init(LED_PIN_ROUGE);
+    gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
+    gpio_put(LED_PIN_ROUGE, 1);
+
+    // Il faut neutraliser cette broche qui pourrait interférer avec 
+    // la lecture des codeurs. (problème sur la carte électrique)...
+    gpio_init(LED_PIN_NE_PAS_UTILISER);
+    gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);
+
+    sleep_ms(3000);
+    Servomoteur_Init();
+    //puts("Debut");
+    //spi_test();
+
+    //while(1);
+    Temps_init();
+    Moteur_Init();
+    QEI_init();
+    AsserMoteur_Init();
+    Localisation_init();
+
+    while(mode_test());
+
+    Gyro_Init();
+
+    
+
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_old = temps_ms;
+    while (1) { 
+        u_int16_t step_ms = 2;
+        float coef_filtre = 1-0.8;
+        
+        while(temps_ms == Temps_get_temps_ms());
+        temps_ms = Temps_get_temps_ms();
+        temps_ms_old = temps_ms;
+        
+        // Tous les pas de step_ms
+        if(!(temps_ms % step_ms)){
+            temps_us_debut_cycle = time_us_32();
+            Gyro_Read(step_ms);
+            
+            //gyro_affiche(gyro_get_vitesse(), "Angle :");
+            // Filtre 
+            angle_gyro = gyro_get_vitesse();
+            if(vitesse_filtre_x == V_INIT){
+                vitesse_filtre_x = angle_gyro.rot_x;
+                vitesse_filtre_y = angle_gyro.rot_y;
+                vitesse_filtre_z = angle_gyro.rot_z;
+            }else{
+                vitesse_filtre_x = vitesse_filtre_x * (1-coef_filtre) + angle_gyro.rot_x * coef_filtre;
+                vitesse_filtre_y = vitesse_filtre_y * (1-coef_filtre) + angle_gyro.rot_y * coef_filtre;
+                vitesse_filtre_z = vitesse_filtre_z * (1-coef_filtre) + angle_gyro.rot_z * coef_filtre;
+            }
+            temps_cycle = time_us_32() - temps_us_debut_cycle;
+
+            //printf("%#x, %#x\n", (double)temps_ms_old / 1000,  vitesse_filtre_z);
+
+            //printf("%d, %d\n", temps_ms, (int32_t) (vitesse_filtre_z * 1000));
+            //gyro_affiche(angle_gyro, "Vitesse (°/s),");
+            
+        }
+
+        // Toutes les 50 ms
+        if((Temps_get_temps_ms() % 50) == 0){
+            struct t_angle_gyro_double m_gyro;
+            m_gyro = gyro_get_angle_degres();
+            printf("%f, %f, %d\n", (double)temps_ms / 1000,  m_gyro.rot_z, temps_cycle);
+        }
+
+        // Toutes les 500 ms
+        if((Temps_get_temps_ms() % 500) == 0){
+            //gyro_affiche(gyro_get_angle(), "Angle :");
+        }
+        // Toutes les secondes
+        if((Temps_get_temps_ms() % 500) == 0){
+            //gyro_get_temp();
+        }
+    }
+}
diff --git a/Robot_config.c b/Robot_config.c
new file mode 100644
index 0000000..873d199
--- /dev/null
+++ b/Robot_config.c
@@ -0,0 +1,9 @@
+int position_avec_gyroscope = 0;
+
+int get_position_avec_gyroscope(void){
+    return position_avec_gyroscope;
+}
+
+void set_position_avec_gyroscope(int _use_gyro){
+    position_avec_gyroscope = _use_gyro;
+}
\ No newline at end of file
diff --git a/Robot_config.h b/Robot_config.h
new file mode 100644
index 0000000..e1675fd
--- /dev/null
+++ b/Robot_config.h
@@ -0,0 +1,2 @@
+int get_position_avec_gyroscope(void);
+void set_position_avec_gyroscope(int _use_gyro);
\ No newline at end of file
diff --git a/Test.c b/Test.c
new file mode 100644
index 0000000..0138d8e
--- /dev/null
+++ b/Test.c
@@ -0,0 +1,652 @@
+#include <stdio.h>
+#include "pico/multicore.h"
+#include "pico/stdlib.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "pico/binary_info.h"
+#include "math.h"
+#include "Test.h"
+
+#include "APDS_9960.h"
+#include "gyro.h"
+#include "Asser_Moteurs.h"
+#include "Asser_Position.h"
+#include "Commande_vitesse.h"
+#include "i2c_maitre.h"
+#include "Localisation.h"
+#include "Moteurs.h"
+#include "QEI.h"
+#include "Robot_config.h"
+#include "Servomoteur.h"
+#include "spi_nb.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+
+#define V_INIT -999.0
+#define TEST_TIMEOUT_US 10000000
+
+int test_APDS9960(void);
+int test_moteurs(void);
+int test_QIE(void);
+int test_QIE_mm(void);
+int test_vitesse_moteur(enum t_moteur moteur);
+int test_asser_moteur(void);
+int test_localisation(void);
+int test_avance(void);
+int test_cde_vitesse_rotation(void);
+int test_cde_vitesse_rectangle(void);
+int test_cde_vitesse_cercle(void);
+int test_asser_position_avance(void);
+int test_asser_position_avance_et_tourne(int);
+int test_trajectoire(void);
+int test_tca9535(void);
+void affiche_localisation(void);
+
+
+// Mode test : renvoie 0 pour quitter le mode test
+int mode_test(){
+    static int iteration = 2;
+    printf("Appuyez sur une touche pour entrer en mode test :\n");
+    printf("A - pour asser_moteurs (rotation)\n");
+    printf("B - pour avance (asser_moteur)\n");
+    printf("C - pour les codeurs\n");
+    printf("D - pour les codeurs (somme en mm)\n");
+    printf("E - Commande en vitesse - rotation pure\n");
+    printf("F - Commande en vitesse - carré\n");
+    printf("G - Commande en vitesse - cercle\n");
+    printf("H - Asser Position - avance\n");
+    printf("I - Asser Position - avance et tourne (gyro)\n");
+    printf("J - Asser Position - avance et tourne (sans gyro)\n");
+    printf("M - pour les moteurs\n");
+    printf("L - pour la localisation\n");
+    printf("T - Trajectoire\n");
+    printf("U - Scan du bus i2c\n");
+    printf("V - APDS_9960\n");
+    stdio_flush();
+    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
+    stdio_flush();
+    switch (rep)
+    {
+    case 'a':
+    case 'A':
+        while(test_asser_moteur());
+        break;
+    case 'b':
+    case 'B':
+        while(test_avance());
+        break;
+        
+    case 'C':
+    case 'c':
+        while(test_QIE());
+        break;
+
+    case 'D':
+    case 'd':
+        while(test_QIE_mm());
+        break;
+
+    case 'E':
+    case 'e':
+        while(test_cde_vitesse_rotation());
+        break;
+
+    case 'F':
+    case 'f':
+        while(test_cde_vitesse_rectangle());
+        break;
+
+    case 'G':
+    case 'g':
+        while(test_cde_vitesse_cercle());
+        break;
+
+    case 'H':
+    case 'h':
+        while(test_asser_position_avance());
+        break;
+
+    case 'I':
+    case 'i':
+        while(test_asser_position_avance_et_tourne(1));
+        break;
+    
+    case 'J':
+    case 'j':
+        while(test_asser_position_avance_et_tourne(0));
+        break;
+        
+    case 'M':
+    case 'm':
+        while(test_moteurs());
+        break;
+    case 'L':
+    case 'l':
+        while(test_localisation());
+        break;
+
+    case 'T':
+    case 't':
+        while(test_trajectoire());
+        break;
+
+    case 'U':
+    case 'u':
+        while(test_tca9535());
+        break;
+
+    case 'V':
+    case 'v':
+        while(test_APDS9960());
+        break;
+
+    case PICO_ERROR_TIMEOUT:
+        iteration--;        
+        if(iteration == 0){
+            printf("Sortie du mode test\n");
+            return 0;
+        }
+
+    default:
+        printf("Commande inconnue\n");
+        break;
+    }
+    return 1;
+    
+}
+
+bool reserved_addr(uint8_t addr) {
+    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
+}
+
+int test_APDS9960(){
+    int lettre;
+
+    printf("Initialisation\n");
+    APDS9960_Init();
+    printf("Lecture...\n");
+    /*
+    do{
+        APDS9960_Lire();
+        lettre = getchar_timeout_us(0);
+        stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);*/
+    while(1){
+        APDS9960_Lire();
+        sleep_ms(100);
+    }
+    return 1;
+}
+
+int test_tca9535(){
+    // Adresse I2C : 0b0100 000 R/W
+    // Lecture des broches sur les registres 0 et 1
+    // Registre 2 et 3 : valeur des broches en sorties
+    // Registre 4 et 5 : INversion de polarité
+    // Registre 6 et 7 : Configuration entrée (1) ou sortie (0)
+
+    uint8_t reception[8];
+    uint8_t emission[8];
+    //uint8_t adresse = 0b0100000;
+    uint8_t adresse = 0x39;
+    int statu;
+    int lettre;
+
+    emission[0]=6; // Registre à lire
+
+    i2c_maitre_init();
+    // Scan bus I2C - cf SDK
+    printf("\nI2C Bus Scan\n");
+    printf("   0   1   2   3   4   5   6   7   8   9   A   B   C   D   E   F\n");
+    for (int addr = 0; addr < (1 << 7); ++addr) {
+        if (addr % 16 == 0) {
+            printf("%02x ", addr);
+        }
+        int ret;
+        uint8_t rxdata;
+        if (reserved_addr(addr))
+            ret = PICO_ERROR_GENERIC;
+        else
+            ret = i2c_read_blocking(i2c_default, addr, &rxdata, 1, false);
+        
+        printf(ret < 0 ? "." : "@");
+        printf(addr % 16 == 15 ? "\n" : " ");
+    }
+    printf("Done.\n");
+    return 0;
+
+    do{    
+        statu = i2c_write_blocking (i2c0, adresse, emission, 1, 0);
+        if(statu == PICO_ERROR_GENERIC){
+            printf("Emission : Address not acknowledged, no device present.\n");
+            return 0;
+        }else{
+            printf("Emission : Ok\n");
+        }
+
+        statu = i2c_read_blocking(i2c0, adresse, reception, 2, 0);
+        if(statu == PICO_ERROR_GENERIC){
+            printf("Reception : Address not acknowledged, no device present.\n");
+            return 0;
+        }else{
+            printf("Recetion : Ok\n");
+        }
+        printf("%2.x%2.x\n",reception[0], reception[1]);
+        
+        lettre = getchar_timeout_us(0);
+        stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+
+}
+
+void test_trajectoire_printf(){
+    struct position_t _position;
+    while(1){
+        _position  = Trajet_get_consigne();
+        printf("T: %ld, X: %f, Y: %f, orientation: %2.1f\n", time_us_32()/1000, _position.x_mm, _position.y_mm, _position.angle_radian/M_PI*180);
+    }
+
+}
+
+int test_trajectoire(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    Trajet_init();
+    struct trajectoire_t trajectoire;
+    printf("Choix trajectoire :\n");
+    printf("B - Bezier\n");
+    printf("C - Circulaire\n");
+    printf("D - Droite\n");
+    do{
+         lettre = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    switch(lettre){
+        case 'b':
+        case 'B':
+            Trajectoire_bezier(&trajectoire, 0, 0, -200., 450, 250, 450, 0, 0);
+            break;
+
+        case 'c':
+        case 'C':
+            Trajectoire_circulaire(&trajectoire, 0, 250, -90, 90, 250);
+            break;
+
+        case 'd':
+        case 'D':
+            Trajectoire_droite(&trajectoire, 0, 0, 0, 700);
+            break;
+        
+        default: return 0;
+    }
+
+    Trajet_debut_trajectoire(trajectoire);
+    multicore_launch_core1(test_trajectoire_printf);
+    do{
+        // Routines à 1 ms
+        QEI_update();
+        Localisation_gestion();
+        
+        if(Trajet_avance(_step_ms/1000.) == TRAJET_TERMINE){
+            Moteur_SetVitesse(MOTEUR_A, 0);
+            Moteur_SetVitesse(MOTEUR_B, 0);
+            Moteur_SetVitesse(MOTEUR_C, 0);
+        }else{
+            AsserMoteur_Gestion(_step_ms);
+        }
+        sleep_ms(_step_ms);
+        temps_ms += _step_ms;
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+    
+}
+
+/// @brief Avance droit 100 mm/s en tournant sur lui-même (1rad/s)
+/// @param m_gyro : 1 pour utiliser le gyroscope, 0 sans
+/// @return 
+int test_asser_position_avance_et_tourne(int m_gyro){
+    int lettre, _step_ms = 1, _step_ms_gyro = 2, step_gyro=2;
+    uint32_t temps_ms = 0, temps_ms_init = 0, temps_ms_old;
+    struct position_t position_consigne;
+
+    position_consigne.angle_radian = 0;
+    position_consigne.x_mm = 0;
+    position_consigne.y_mm = 0;
+    
+    printf("Le robot avance à 100 mm/s\n");
+    printf("Init gyroscope\n");
+    Gyro_Init();
+    printf("C'est parti !\n");
+    stdio_flush();
+
+    set_position_avec_gyroscope(m_gyro);
+    temps_ms = Temps_get_temps_ms();
+    temps_ms_old = temps_ms;
+    temps_ms_init = temps_ms;
+
+    multicore_launch_core1(affiche_localisation);
+    do{
+        while(temps_ms == Temps_get_temps_ms());
+        temps_ms = Temps_get_temps_ms();
+        temps_ms_old = temps_ms;
+
+        QEI_update();
+        if(temps_ms % _step_ms_gyro == 0){
+            Gyro_Read(_step_ms_gyro);
+        }
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+        
+        position_consigne.angle_radian = (double) (temps_ms - temps_ms_init) /1000.;
+        position_consigne.y_mm = (double) (temps_ms - temps_ms_init) * 100. / 1000.;
+
+        Asser_Position(position_consigne);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0);
+
+    printf("lettre : %c, %d\n", lettre, lettre);
+
+    return 0;
+}
+
+int test_asser_position_avance(){
+    int lettre, _step_ms = 1, temps_ms=0;
+    struct position_t position;
+
+    position.angle_radian = 0;
+    position.x_mm = 0;
+    position.y_mm = 0;
+    
+    printf("Le robot avance à 100 mm/s\n");
+    do{
+        QEI_update();
+        Localisation_gestion();
+        AsserMoteur_Gestion(_step_ms);
+        
+        if(temps_ms < 5000){
+            position.y_mm = (double) temps_ms * 100. / 1000.;
+        }else if(temps_ms < 10000){ 
+            position.y_mm = 1000 - (double) temps_ms * 100. / 1000.;
+        }else{
+            temps_ms = 0;
+        }
+
+        Asser_Position(position);
+        temps_ms += _step_ms;
+        sleep_ms(_step_ms);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
+
+int test_cde_vitesse_rotation(){
+    int lettre, _step_ms = 1;
+    double vitesse =90.0/2 * 3.14159 /180.0;
+    printf("Rotation du robot sur lui-même en 8 secondes\nVitesse : %f rad/s\n", vitesse);
+
+    commande_vitesse(0, 0, vitesse);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
+
+int test_cde_vitesse_rectangle(){
+    int lettre, _step_ms = 1, temps_ms=0;
+
+    printf("déplacement en rectangle du robot : 500x200 mm, 100 mm/s\n");
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+
+        if(temps_ms < 5000){
+            commande_vitesse(0, 100, 0);
+        }else if(temps_ms < 7000){
+            commande_vitesse(-100, 0, 0);
+        }else if(temps_ms < 12000){
+            commande_vitesse(0, -100, 0);
+        }else if(temps_ms < 14000){
+            commande_vitesse(100, 0, 0);
+        }else{
+            temps_ms = 0;
+        }
+        
+        sleep_ms(_step_ms);
+        temps_ms += _step_ms;
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
+
+int test_cde_vitesse_cercle(){
+    int lettre, _step_ms = 1, temps_ms=0;
+
+    printf("déplacement en cercle du robot : 100 mm/s\n");
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        commande_vitesse(cos((double)temps_ms / 1000.) * 200.0, sin((double)temps_ms /1000.) * 200.0, 0);
+        temps_ms += _step_ms;
+        sleep_ms(_step_ms);
+        
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+}
+
+int test_avance(void){
+    int lettre;
+    int _step_ms = 1;
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, -100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0);
+    
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        lettre = getchar_timeout_us(0);
+
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
+    return 0;
+}
+
+void affiche_localisation(){
+    struct position_t position;
+    while(1){
+        position = Localisation_get();
+        printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+
+    }
+}
+
+void test_asser_moteur_printf(){
+    int _step_ms = 1;
+    while(1){
+        printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+            AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        //sleep_ms(5);
+    }
+}
+
+int test_asser_moteur(){
+    int lettre;
+    int _step_ms = 1;
+    printf("Asservissement des moteurs :\nAppuyez sur une touche pour quitter\n");
+    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_B, 100);
+    AsserMoteur_setConsigne_mm_s(MOTEUR_C, 100);
+    multicore_launch_core1(test_asser_moteur_printf);
+    do{
+        QEI_update();
+        AsserMoteur_Gestion(_step_ms);
+        sleep_ms(_step_ms);
+        //printf("Vitesse A : %d, codeur B : %d, codeur C : %d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME), QEI_get(QEI_C_NAME));
+        //printf("Vitesse A : %.0f, vitesse B : %.0f, vitesse C : %.0f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, _step_ms),
+        //    AsserMoteur_getVitesse_mm_s(MOTEUR_B, _step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_C, _step_ms));
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    Moteur_SetVitesse(MOTEUR_A, 0);
+    Moteur_SetVitesse(MOTEUR_B, 0);
+    Moteur_SetVitesse(MOTEUR_C, 0);
+    multicore_reset_core1();
+    return 0;
+}
+
+int test_QIE(){
+    int lettre;
+    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
+    do{
+        QEI_update();
+        printf("Codeur A : %d (%3.2f mm), codeur B : %d (%3.2f mm), codeur C : %d (%3.2f mm)\n", 
+            QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME),
+            QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME),
+            QEI_get(QEI_C_NAME), QEI_get_mm(QEI_C_NAME));
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    return 0;
+
+}
+
+int test_QIE_mm(){
+    int lettre;
+    printf("Affichage des QEI :\nAppuyez sur une touche pour quitter\n");
+    double a_mm=0, b_mm=0, c_mm=0;
+    do{
+        QEI_update();
+        a_mm += QEI_get_mm(QEI_A_NAME);
+        b_mm += QEI_get_mm(QEI_B_NAME);
+        c_mm += QEI_get_mm(QEI_C_NAME);
+        printf("Codeur A : %3.2f mm, codeur B : %3.2f mm, codeur C : %3.2f mm\n", a_mm, b_mm, c_mm);
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+    return 0;
+
+}
+
+int test_localisation(){
+    int lettre;
+    struct position_t position;
+    
+    printf("Affichage de la position du robot.\nAppuyez sur une touche pour quitter\n");
+    do{
+        QEI_update();
+        Localisation_gestion();
+        position = Localisation_get();
+        printf("X: %f, Y: %f, angle: %f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+        sleep_ms(100);
+
+        lettre = getchar_timeout_us(0);
+    }while(lettre == PICO_ERROR_TIMEOUT);
+
+    return 0;
+    
+}
+
+int test_moteurs(){
+    int lettre_moteur;
+
+    printf("Indiquez le moteurs à tester (A, B ou C):\n");
+    do{
+         lettre_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
+         stdio_flush();
+    }while(lettre_moteur == PICO_ERROR_TIMEOUT);
+    printf("Moteur choisi : %c %d %x\n", lettre_moteur, lettre_moteur, lettre_moteur);
+
+    switch (lettre_moteur)
+    {
+    case 'A':
+    case 'a':
+        while(test_vitesse_moteur(MOTEUR_A));
+        break;
+
+    case 'B':
+    case 'b':
+        while(test_vitesse_moteur(MOTEUR_B));
+        break;
+
+    case 'C':
+    case 'c':
+        while(test_vitesse_moteur(MOTEUR_C));
+        break;
+    
+    case 'Q':
+    case 'q':
+        return 0;
+        break;
+    
+    default:
+        break;
+    }
+    
+    return 1;
+}
+
+int test_vitesse_moteur(enum t_moteur moteur){
+    printf("Vitesse souhaitée :\n0 - 0%%\n1 - 10%%\n2 - 20%%\n...\n9 - 90%%\nA - 100%%\n");
+
+    int vitesse_moteur;
+    do{ 
+        vitesse_moteur = getchar_timeout_us(TEST_TIMEOUT_US);
+        stdio_flush();
+    }while(vitesse_moteur == PICO_ERROR_TIMEOUT);
+    
+    switch (vitesse_moteur)
+    {
+    case '0':
+    case '1':
+    case '2':
+    case '3':
+    case '4':
+    case '5':
+    case '6':
+    case '7':
+    case '8':
+    case '9':
+        printf("Vitesse choisie : %c0%%\n", vitesse_moteur);
+        Moteur_SetVitesse(moteur, (vitesse_moteur - '0') * 32767.0 / 10.);
+        break;
+
+    case 'A':
+    case 'a':
+        printf("Vitesse choisie : 100%%\n");
+        Moteur_SetVitesse(moteur, (int16_t) 32766.0);
+        break;
+
+    case 'b':
+    case 'B':
+        printf("Vitesse choisie : -50%%\n");
+        Moteur_SetVitesse(moteur, (int16_t) -32766.0/2);
+        break;
+
+    case 'q':
+    case 'Q':
+        return 0;
+        break;
+
+    default:
+        break;
+    }
+    return 1;
+}
diff --git a/Test.h b/Test.h
new file mode 100644
index 0000000..019bb5e
--- /dev/null
+++ b/Test.h
@@ -0,0 +1 @@
+int mode_test();
\ No newline at end of file
diff --git a/i2c_maitre.c b/i2c_maitre.c
new file mode 100644
index 0000000..185db44
--- /dev/null
+++ b/i2c_maitre.c
@@ -0,0 +1,61 @@
+#include "i2c_maitre.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "pico/stdlib.h"
+#include <stdio.h>
+
+#define I2C_SDA_PIN 16
+#define I2C_SCL_PIN 17
+
+void i2c_maitre_init(void){
+    stdio_init_all();
+    i2c_init(i2c0, 100 * 1000);
+
+    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+
+    gpio_set_function(6, GPIO_FUNC_NULL);
+    gpio_set_function(7, GPIO_FUNC_NULL);
+
+}
+
+/// @brief Pour l'instant bloquant, mais devrait passer en non bloquant bientôt
+/// @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 (i2c0, adresse_7_bits, emission, 1, 0);
+    if(statu == PICO_ERROR_GENERIC){
+        printf("I2C - Envoi registre Echec\n");
+        return I2C_ECHEC;
+    }
+
+    statu = i2c_read_blocking (i2c0, adresse_7_bits, reception, len, 0);
+    if(statu == PICO_ERROR_GENERIC){
+        printf("I2C - Lecture registre Echec\n");
+        return I2C_ECHEC;
+    }
+
+    return I2C_SUCCES;
+}
+
+int i2c_ecrire_registre(char adresse_7_bits, char registre, char valeur_registre){
+    int statu;
+    char emission[2];
+
+    emission[0] = registre;
+    emission[1] = valeur_registre;
+    statu = i2c_write_blocking (i2c0, 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;
+}
\ No newline at end of file
diff --git a/i2c_maitre.h b/i2c_maitre.h
new file mode 100644
index 0000000..a1ad172
--- /dev/null
+++ b/i2c_maitre.h
@@ -0,0 +1,9 @@
+#define I2C_EN_COURS 0
+#define I2C_SUCCES 1
+#define I2C_ECHEC -1
+
+
+
+void i2c_maitre_init(void);
+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);
\ No newline at end of file