diff --git a/SelectionCapteur.c b/SelectionCapteur.c
index 376f14e..afc8dcc 100644
--- a/SelectionCapteur.c
+++ b/SelectionCapteur.c
@@ -1,4 +1,5 @@
 #include "pico/stdlib.h"
+#include "SelectionCapteur.h"
 
 #define PIN_ADRESSE_A0 5
 #define PIN_ADRESSE_A1 4
@@ -16,21 +17,37 @@ void Selection_capteur_init(void){
     Selection_capteur_init_pin_low(PIN_ADRESSE_A1);
     Selection_capteur_init_pin_low(PIN_ADRESSE_A2);
     Selection_capteur_init_pin_low(PIN_ADRESSE_A3);
+    Selection_capteur_deselect();
 
 }
 
 /// @brief Désactive le capteur en question
 /// @param capteur capteur, numéroté de 1 à 12
 void Selection_capteur_select(uint32_t capteur){
+    uint32_t io_value;
     capteur = capteur - 1;
-    gpio_put(PIN_ADRESSE_A0, capteur & 0x01);
+    /*gpio_put(PIN_ADRESSE_A0, capteur & 0x01);
     gpio_put(PIN_ADRESSE_A1, (capteur & 0x02) >> 1);
-    gpio_put(PIN_ADRESSE_A2, (capteur & 0x02) >> 2);
-    gpio_put(PIN_ADRESSE_A3, (capteur & 0x04) >> 3);
+    gpio_put(PIN_ADRESSE_A2, (capteur & 0x04) >> 2);
+    gpio_put(PIN_ADRESSE_A3, (capteur & 0x08) >> 3);*/
+
+    io_value = 0;
+    io_value |= ((capteur & 0x08) >> 3) << 2;
+    io_value |= ((capteur & 0x04) >> 2) << 3;
+    io_value |= ((capteur & 0x02) >> 1) << 4;
+    io_value |= (capteur & 0x01) << 5;
+
+    printf("io_value : %x, capteur : %d\n", io_value, capteur);
+
+    gpio_put_masked(0b111100, io_value);
 }
 
 void Selection_capteur_init_pin_low(uint pin){
     gpio_init(pin);
     gpio_set_dir(pin, GPIO_OUT);
     gpio_put(pin, 0);
+}
+
+void Selection_capteur_deselect(){
+    Selection_capteur_select(15);
 }
\ No newline at end of file
diff --git a/SelectionCapteur.h b/SelectionCapteur.h
index 57897b1..7ec60ad 100644
--- a/SelectionCapteur.h
+++ b/SelectionCapteur.h
@@ -1,4 +1,5 @@
 #include "pico/stdlib.h"
 
 void Selection_capteur_init(void);
-void Selection_capteur_select(uint32_t capteur);
\ No newline at end of file
+void Selection_capteur_select(uint32_t capteur);
+void Selection_capteur_deselect(void);
\ No newline at end of file
diff --git a/VL53L1X_api.c b/VL53L1X_api.c
index 572040d..fd62b6e 100644
--- a/VL53L1X_api.c
+++ b/VL53L1X_api.c
@@ -229,6 +229,9 @@ VL53L1X_ERROR VL53L1X_SensorInit(uint16_t dev)
 
 	for (Addr = 0x2D; Addr <= 0x87; Addr++){
 		status |= VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]);
+		if(status){
+			return status;
+		}
 	}
 	status |= VL53L1X_StartRanging(dev);
 	tmp  = 0;
diff --git a/main.c b/main.c
index 9d1885b..4899698 100644
--- a/main.c
+++ b/main.c
@@ -19,6 +19,11 @@ int continuous_reading(uint8_t device);
 int calibration(uint8_t device);
 int change_address(uint8_t * device, uint8_t new_i2c_7bits_address);
 void initialise_adresses(void);
+void init_sensors_1_et_2(void);
+void init_sensors(void);
+void test_changement_adresse_1(void);
+void test_changement_adresse_2(void);
+void test_changement_adresse_3(void);
 void display_menu();
 
 
@@ -32,7 +37,7 @@ void main(void)
     stdio_init_all();
     i2c_master_init();
     Selection_capteur_init();
-    Selection_capteur_select(1);
+    //Selection_capteur_select(1);
 
     ws2812_init();
 
@@ -48,22 +53,49 @@ void main(void)
                 display_menu();
             }
         }while(keycode == PICO_ERROR_TIMEOUT || keycode == 0);
+        answer_at_least_once = 1;
+
         switch (keycode)
         {
         case 'a':
         case 'A':
-            while(change_address(&VL53L1X_device, VL53L1X_device + 3));
+            printf("Changement d'adresse\n");
+            change_address(&VL53L1X_device, VL53L1X_device + 3);
             printf("New address: %d\n", VL53L1X_device);
             break;
+        case 'i':
+        case 'I':
+            printf("Initialisation des adresses\n");
+            initialise_adresses();
+            printf("Initialisation des capteurs\n");
+            init_sensors();
+            break;
+        case 'j':
+        case 'J':
+            init_sensors_1_et_2();
+            break;
+        case 'k':
+        case 'K':
+            ws2812_arc_en_ciel();
+            break;
+        case 'l':
+        case 'L':
+            test_changement_adresse_1();
+            break;
+        case 'm':
+        case 'M':
+            test_changement_adresse_2();
+            break;
+        case 'n':
+        case 'N':
+            test_changement_adresse_3();
+            break;
         case 'o':
         case 'O':
             while(calibration(VL53L1X_device));
-            answer_at_least_once=1;
             break;
-        
         case 'r':
         case 'R':
-            answer_at_least_once=1;
             while(continuous_reading(VL53L1X_device));
             break;
         default :
@@ -75,15 +107,383 @@ void main(void)
 }
 
 void initialise_adresses(void){
-    for(uint capteur=1; capteur<=12; capteur++){
-        Selection_capteur_select(1);
+    uint8_t VL53L1X_device = 0x29;
+    for(uint capteur=1; capteur<12; capteur++){
+        Selection_capteur_select(capteur);
+        if(change_address(&VL53L1X_device, VL53L1X_device+1)){
+            printf("Erreur change adresse : %x => %x, capteur : %d\n", VL53L1X_device, VL53L1X_device+1, capteur);
+            VL53L1X_device++;
+        }else{
+            printf("change adresse : %x => %x, sauf capteur : %d\n", VL53L1X_device-1, VL53L1X_device, capteur);
+        }
+    }
+}
+
+void init_sensors(void){
+    uint8_t VL53L1X_device = 0x29;
+    uint8_t adresse;
+    Selection_capteur_deselect();
+    for(uint capteur=0; capteur<12; capteur++){
+        adresse = VL53L1X_device + capteur;
+        if(VL53L1X_SensorInit(adresse)){
+            // Bad init
+            ws2812_set_buffer_rgb(1,0,0, capteur);
+            printf("Init KO : capteur %d, adresse %x\n", capteur, adresse);
+        }else{
+            // Good init
+            ws2812_set_buffer_rgb(0,1,0, capteur);
+            printf("Init ok : capteur %d, adresse %x\n", capteur, adresse);
+        }
+    }
+    ws2812_affiche_buffer();
+}
+
+void init_sensors_1_et_2(void){
+    const uint8_t tmp_i2c_adresse = 0x28;
+    const uint8_t default_i2c_adresse = 0x29;
+    uint8_t VL53L1X_device_1 = default_i2c_adresse;
+    uint8_t VL53L1X_device_2 = default_i2c_adresse;
+    uint8_t VL53L1X_device_3 = default_i2c_adresse;
+    Selection_capteur_deselect();    
+    sleep_ms(10);
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, 0x30);
+    if(change_address(&VL53L1X_device_1, 0x30)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
     }
     
+    /* // réinitialisation du capteur 1.
+    printf("Capteur #1 par defaut : 0x29\n");
+    Selection_capteur_select(1);
+    sleep_ms(1);
+    Selection_capteur_deselect();    
+    sleep_ms(1);
+    VL53L1X_device_1 = 0x29;*/
+
+    /*printf("Changement d'adresse capteur #1 %x => %x\n", VL53L1X_device_1, 0x30);
+    if(change_address(&VL53L1X_device_1, 0x30)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }*/
+
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+
+    Selection_capteur_select(1);
+    sleep_ms(1);
+    Selection_capteur_deselect();    
+    sleep_ms(1);
+
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+
+    if(VL53L1X_SensorInit(default_i2c_adresse)){
+        printf("Init KO : adresse %x\n", default_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", default_i2c_adresse);
+    }
+
+
+    /* // réinitialisation du capteur 2.
+    Selection_capteur_select(2);
+    sleep_ms(1);
+    Selection_capteur_deselect();    
+    sleep_ms(1);
+    VL53L1X_device_2 = 0x29;
+
+    printf("Changement d'adresse capteur #2 %x => %x\n", VL53L1X_device_2, 0x31);
+    if(change_address(&VL53L1X_device_2, 0x31)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }*/
 }
 
+void test_changement_adresse_1(void){
+    const uint8_t tmp_i2c_adresse = 0x28;
+    const uint8_t default_i2c_adresse = 0x29;
+    uint8_t VL53L1X_device_1 = default_i2c_adresse;
+    uint8_t VL53L1X_device_2 = default_i2c_adresse;
+    uint8_t VL53L1X_device_3 = default_i2c_adresse;
+    Selection_capteur_deselect();    
+    sleep_ms(10);
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, 0x30);
+    if(change_address(&VL53L1X_device_1, 0x30)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    /*if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }*/
+
+    VL53L1X_device_1 = 0x29;
+
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
+    if(change_address(&VL53L1X_device_1, 0x31)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+    if(VL53L1X_SensorInit(0x30)){
+        printf("Init KO : adresse %x\n", 0x30);
+    }else{
+        printf("Init OK : adresse %x\n", 0x30);
+    }
+    if(VL53L1X_SensorInit(0x31)){
+        printf("Init KO : adresse %x\n", 0x31);
+    }else{
+        printf("Init OK : adresse %x\n", 0x31);
+    }
+}
+
+void test_changement_adresse_2(void){
+    const uint8_t tmp_i2c_adresse = 0x30;
+    const uint8_t default_i2c_adresse = 0x29;
+    uint8_t VL53L1X_device_1 = default_i2c_adresse;
+    uint8_t VL53L1X_device_2 = default_i2c_adresse;
+    uint8_t VL53L1X_device_3 = default_i2c_adresse;
+    Selection_capteur_deselect();    
+    sleep_ms(10);
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, tmp_i2c_adresse);
+    if(change_address(&VL53L1X_device_1, tmp_i2c_adresse)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_2, tmp_i2c_adresse);
+    if(change_address(&VL53L1X_device_2, tmp_i2c_adresse)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    // Réinitialisation du capteur 1.
+    printf("Capteur #1 par defaut : 0x29\n");
+    Selection_capteur_select(1);
+    sleep_ms(1);
+    Selection_capteur_deselect();    
+    sleep_ms(1);
+    VL53L1X_device_1 = 0x29;
+
+
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
+    if(change_address(&VL53L1X_device_1, 0x31)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    /* // Réinitialisation du capteur 2.
+    printf("Capteur #2 par defaut : 0x29\n");
+    Selection_capteur_select(2);
+    sleep_ms(1);
+    Selection_capteur_deselect();    
+    sleep_ms(1);
+
+    VL53L1X_device_2 = 0x29;
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_2, 0x32);
+    if(change_address(&VL53L1X_device_2, 0x32)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }*/
+
+    
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_2)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_2);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_2);
+    }
+    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
+        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
+    }
+}
+
+void test_changement_adresse_3(void){
+    const uint8_t tmp_i2c_adresse = 0x30;
+    const uint8_t default_i2c_adresse = 0x29;
+    uint8_t VL53L1X_device_1 = default_i2c_adresse;
+    uint8_t VL53L1X_device_2 = default_i2c_adresse;
+    uint8_t VL53L1X_device_3 = default_i2c_adresse;
+    Selection_capteur_deselect();    
+    sleep_ms(10);
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, tmp_i2c_adresse);
+    if(change_address(&VL53L1X_device_1, tmp_i2c_adresse)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_2, tmp_i2c_adresse);
+    if(change_address(&VL53L1X_device_2, tmp_i2c_adresse)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    // Réinitialisation du capteur 1.
+    printf("Capteur #1 par defaut : 0x29\n");
+    Selection_capteur_select(1);
+    sleep_ms(1);
+    Selection_capteur_deselect();   
+    sleep_ms(1);
+    VL53L1X_device_1 = 0x29;
+
+
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
+    if(change_address(&VL53L1X_device_1, 0x31)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+     // Réinitialisation du capteur 3.
+    printf("Capteur #2 par defaut : 0x29\n");
+    Selection_capteur_select(3);
+    sleep_ms(1);
+    Selection_capteur_deselect();
+    sleep_ms(1);
+
+    VL53L1X_device_3 = 0x29;
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_3, 0x33);
+    if(change_address(&VL53L1X_device_3, 0x33)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    if(VL53L1X_SensorInit(default_i2c_adresse)){
+        printf("Init KO : adresse %x\n", default_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", default_i2c_adresse);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_2)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_2);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_2);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_3)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_3);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_3);
+    }
+    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
+        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
+    }
+
+
+
+
+     // Réinitialisation du capteur 2.
+    printf("Capteur #2 par defaut : 0x29\n");
+    Selection_capteur_select(2);
+    sleep_ms(1);
+
+    if(VL53L1X_SensorInit(default_i2c_adresse)){
+        printf("Init KO : adresse %x\n", default_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", default_i2c_adresse);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_2)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_2);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_2);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_3)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_3);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_3);
+    }
+    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
+        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
+    }
+
+
+
+    Selection_capteur_deselect();
+    sleep_ms(1);
+
+    VL53L1X_device_2 = 0x29;
+    printf("Changement d'adresse %x => %x\n", VL53L1X_device_2, 0x32);
+    if(change_address(&VL53L1X_device_2, 0x32)){
+        printf("-> KO\n");
+    }else{
+        printf("-> ok;\n");
+    }
+
+    
+    if(VL53L1X_SensorInit(default_i2c_adresse)){
+        printf("Init KO : adresse %x\n", default_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", default_i2c_adresse);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_1)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_1);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_1);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_2)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_2);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_2);
+    }
+    if(VL53L1X_SensorInit(VL53L1X_device_3)){
+        printf("Init KO : adresse %x\n", VL53L1X_device_3);
+    }else{
+        printf("Init OK : adresse %x\n", VL53L1X_device_3);
+    }
+    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
+        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
+    }else{
+        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
+    }
+
+}
+
+
 void display_menu(){
     printf("Select action :\n");
     printf("A - Change I2C address\n");
+    printf("I - Init I2C address\n");
+    printf("J - Init catpeur 1 et 2\n");
+    printf("K - Arc en ciel\n");
+    printf("L - Test changement d'adresse, 1 capteur\n");
+    printf("M - Test changement d'adresse, 2 capteurs\n");
+    printf("N - Test changement d'adresse, 3 capteurs\n");
     printf("O - Offset Calibration\n");
     printf("R - Read distance\n");
 }
@@ -92,11 +492,11 @@ int change_address(uint8_t *device, uint8_t new_i2c_7bits_address){
     int status;
     status = VL53L1X_SetI2CAddress(*device, new_i2c_7bits_address << 1);
     if(status){
-        printf("VL53L1X_SetI2CAddress, Error :%d\n", status);
+        //printf("VL53L1X_SetI2CAddress, Error :%d\n", status);
     }else{
         *device=new_i2c_7bits_address;
     }
-    return 0;
+    return status;
 }
 
 int calibration(uint8_t device){
diff --git a/vl53l1_platform.c b/vl53l1_platform.c
index 10fa7ea..8135da8 100644
--- a/vl53l1_platform.c
+++ b/vl53l1_platform.c
@@ -54,6 +54,7 @@ int8_t i2c_write_register(char adresse_7_bits, uint16_t index, uint8_t * values,
     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;
@@ -69,11 +70,17 @@ int8_t i2c_write_register(char adresse_7_bits, uint16_t index, uint8_t * values,
 		buffer[2+i] = values[i];
 	}
 
-    statu = i2c_write_blocking (i2c0, adresse_7_bits, buffer, count + 2, 0);
+	// Define timeout - now + 1s.
+	timeout_time = time_us_64() + 1000000;
+    statu = i2c_write_blocking_until (i2c0, adresse_7_bits, buffer, count + 2, 0, timeout_time);
+	//statu = i2c_write_blocking (i2c0, adresse_7_bits, buffer, count + 2, 0);
     if(statu == PICO_ERROR_GENERIC){
-        printf("Erreur ecrire registre\n");
+        //printf("Erreur ecrire registre\n");
         return I2C_FAILED;
-    }
+    }else if(statu == PICO_ERROR_TIMEOUT){
+		printf("Erreur ecrire registre: timeout\n");
+		return I2C_FAILED;
+	}
 
     return I2C_SUCCESS;
 }