diff --git a/prog/prog.ino b/prog/prog.ino
index 87253d5..1628665 100644
--- a/prog/prog.ino
+++ b/prog/prog.ino
@@ -1,53 +1,19 @@
 #include <ESP32Servo.h>
 #include <Wire.h>
-#include "Adafruit_VL53L0X.h"
+#include <VL53L0X.h>
 #include <Adafruit_PCF8574.h>
 #include <WiFi.h>
 #include <AsyncTCP.h>
 #include <ESPAsyncWebServer.h>
 
-/*
-  #include "terminal.hpp"
-  //
-  // ATTENTION : pour que les commandes se déclarent automatiquement au démarrage
-  // auprès du terminal, le fichier conteant la fonction setup() et loop() doit 
-  // être mis en derniere position dans la liste des fichiers lors de l'édition 
-  // des liens. 
-  // Dans platformio, pour faire cela, il suffit de commencer le nom du fichier 
-  // contenant les fonctions loop() et setup() par la lettre z. 
-  // Par exemple : zzz_main.cpp.
-  //
+// Web server configuration
+const char* ssid = "ESP_ping";
+const char* password = "pong";
 
-  extern terminal_n::Terminal terminal;
-
-
-  TERMINAL_PARAMETER_INT(
-    terminal,
-    power_mode,            // variable name to declare
-    "power_mode",          // EEPROM name (15 char. max), 
-                          // If you dont want to save in EEPROM, set ""
-    "the power mode",      // description
-    0,                     // default constructor value 
-    false,                      // auto save in eeprom after command modification
-    terminal_n::filter_nothing, // function to filter the value
-    terminal_n::do_nothing     // function executed at the end of the command
-  );
-
-  TERMINAL_COMMAND(terminal, ping, "Print pong followoing by the received arguments")
-  {
-    terminal.print("pong");
-    for (uint32_t i=0; i<argc; i++) {
-      terminal.print(" ");
-      terminal.print(argv[i]);
-    }
-    terminal.println("");
-  }
-  TERMINAL_ADD_DEFAULT_COMMANDS(terminal) // help, params, load_config, reset_config, echo ...
-
-  terminal_n::Terminal terminal("Pingpong flottant.\n\r\n\r");
-
-*/
+const char* PARAM_INPUT_1 = "output";
+const char* PARAM_INPUT_2 = "state";
 
+AsyncWebServer server(80);
 
 struct pin {
     int id;
@@ -55,8 +21,8 @@ struct pin {
 };
 
 struct tube {
-  float height;
-  float target;
+  uint16_t height;
+  uint16_t target;
   struct {
     Servo hard;
     struct pin pin;
@@ -64,32 +30,114 @@ struct tube {
     int equilibrium;
   } servo;
   struct {
-    Adafruit_VL53L0X hard;
+    VL53L0X hard;
     struct pin gpio;
     struct pin xshut;
   } tof;
   int id;
 };
 
+const struct pin all_xshuts[6] = {
+    {.id = 17, .is_extender = 0},
+    {.id = 2, .is_extender = 1},
+    {.id = 3, .is_extender = 1},
+    {.id = 7, .is_extender = 1},
+    {.id = 5, .is_extender = 1},
+    {.id = 1, .is_extender = 1}
+};
 
-const int TUBES_COUNT = 6;
-struct tube all_tubes[6] = {0};
+const struct pin all_gpios[6] = {
+    {.id = 5, .is_extender = 0},
+    {.id = 18, .is_extender = 0},
+    {.id = 19, .is_extender = 0},
+    {.id = -1, .is_extender = 1},
+    {.id = 6, .is_extender = 1},
+    {.id = 0, .is_extender = 1}
+};
+
+const struct pin all_servos[6] = {
+    {.id = 32, .is_extender = 0},
+    {.id = 33, .is_extender = 0},
+    {.id = 23, .is_extender = 0},
+    {.id = 16, .is_extender = 0},
+    {.id = 14, .is_extender = 0},
+    {.id = 2, .is_extender = 0}
+};
+
+struct tube all_tubes[6];
+const int TUBES_COUNT = 1;
 
 const float minHeight = 50;
 const float maxHeight = 445;
 
-int minServo = 0;
+int minServo = 50;
 int maxServo = 180;
 
-// Replace with your network credentials
-const char* ssid = "ESP_ping";
-const char* password = "pong";
 
-const char* PARAM_INPUT_1 = "output";
-const char* PARAM_INPUT_2 = "state";
+Adafruit_PCF8574 pcf8574;
 
-// Create AsyncWebServer object on port 80
-AsyncWebServer server(80);
+void scan_I2c(){
+  byte error, address;
+  int nDevices;
+  Serial.println("Scanning...");
+  nDevices = 0;
+  for(address = 1; address < 127; address++ ) {
+    Wire.beginTransmission(address);
+    error = Wire.endTransmission();
+    if (error == 0) {
+      Serial.print("I2C device found at address 0x");
+      if (address<16) {
+        Serial.print("0");
+      }
+      Serial.println(address,HEX);
+      nDevices++;
+    }
+    else if (error==4) {
+      Serial.print("Unknow error at address 0x");
+      if (address<16) {
+        Serial.print("0");
+      }
+      Serial.println(address,HEX);
+    }    
+  }
+  if (nDevices == 0) {
+    Serial.println("No I2C devices found\n");
+  }
+  else {
+    Serial.println("done\n");
+  }
+  delay(5000);
+}
+
+uint16_t mesureHeight(int i){
+  uint16_t h = all_tubes[i].tof.hard.readRangeContinuousMillimeters();
+  //Serial.printf("measure: %d\n", h);
+  if (h > 2000){
+    //Serial.printf("returning height\n");
+    return all_tubes[i].height;
+  } else {
+    //Serial.printf("returning: %d\n", h);
+    return h;
+  }
+}
+
+void change_pin(struct pin pin, int state) {
+    if (pin.is_extender) {
+        pcf8574.digitalWrite(pin.id, state);
+    } else {
+        digitalWrite(pin.id, state);
+    }
+}
+
+void changePinMode(struct pin pin, int state) {
+    if (pin.is_extender) {
+        pcf8574.pinMode(pin.id, state);
+    } else {
+        pinMode(pin.id, state);
+    }
+}
+
+// Web Server configuration 
 
 const char index_html[] PROGMEM = R"rawliteral(
   <!DOCTYPE HTML><html>
@@ -128,7 +176,6 @@ const char index_html[] PROGMEM = R"rawliteral(
 )rawliteral";
 
 
-// Replaces placeholder with button section in your web page
 String processor(const String& var){
   //Serial.println(var);
   if(var == "BUTTONPLACEHOLDER"){
@@ -146,26 +193,6 @@ String processor(const String& var){
 }
 
 
-auto mesureHeight(Adafruit_VL53L0X lox, struct tube *tube){
-  VL53L0X_RangingMeasurementData_t measure;
-  lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
-  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
-    float height = measure.RangeMilliMeter;
-    if (height < 1000.0){
-      tube->height = height;
-      return height;
-    } else {
-      Serial.println("Incoherent measurement. Returning previous one.");
-      return tube->height;
-    }
-  }
-  else {
-    //Serial.println(" out of range ");
-    return tube->height;
-  }
-}
-
-
 void wifisetup() {
   Serial.print("Setting AP (Access Point)…");
   WiFi.softAP(ssid);
@@ -181,17 +208,19 @@ void wifisetup() {
 
   // Send a GET request to <ESP_IP>/update?output=<inputMessage1>&state=<inputMessage2>
   server.on("/update", HTTP_GET, [] (AsyncWebServerRequest *request) {
-    String inputMessage1;
-    String inputMessage2;
+    int inputMessage1;
+    int inputMessage2;
     // GET input1 value on <ESP_IP>/update?output=<inputMessage1>&state=<inputMessage2>
     if (request->hasParam(PARAM_INPUT_1) && request->hasParam(PARAM_INPUT_2)) {
-      inputMessage1 = request->getParam(PARAM_INPUT_1)->value();
-      inputMessage2 = request->getParam(PARAM_INPUT_2)->value();
-      all_tubes[0].target = inputMessage2.toFloat();
+      inputMessage1 = request->getParam(PARAM_INPUT_1)->value().toInt();
+      inputMessage2 = request->getParam(PARAM_INPUT_2)->value().toInt();
+      Serial.printf("Received web command: %d %d\n", inputMessage1, inputMessage2);
+      all_tubes[inputMessage1].target = inputMessage2;
     }
     else {
-      inputMessage1 = "No message sent";
-      inputMessage2 = "No message sent";
+      // Error 
+      inputMessage1 = -1;
+      inputMessage2 = -1;
     }
     Serial.print("GPIO: ");
     Serial.print(inputMessage1);
@@ -204,128 +233,63 @@ void wifisetup() {
   server.begin();
 }
 
-Adafruit_PCF8574 pcf8574;
-
-void change_pin(struct pin pin, int state) {
-    if (pin.is_extender) {
-        pcf8574.digitalWrite(pin.id, state);
-    } else {
-        digitalWrite(pin.id, state);
-    }
-}
-
-void changePinMode(struct pin pin, int state) {
-    if (pin.is_extender) {
-        pcf8574.pinMode(pin.id, state);
-    } else {
-        pinMode(pin.id, state);
-    }
-}
-
-Adafruit_VL53L0X tofs_talk_through[6] = {Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X()};
-
-
-
-void init_tube(struct tube *tube, char id, struct pin servo_pin, struct pin gpio, struct pin xshut) {
-  Serial.print(" Initialiazing tube #");
-  Serial.println((int) id);
-  tube->id = id;
-  tube->servo.pin = servo_pin;
-  tube->tof.gpio = gpio;
-  tube->tof.xshut = xshut;
-  tube->height = 250.0f;
-  tube->target = 250.0f;
-  change_pin(gpio, HIGH);
-  change_pin(xshut, HIGH);
-  //change_pin(servo_pin, HIGH);
-  Serial.println("Beginning boot sequence VL53L0X");
-  /*
-  if (!tofs_talk_through[id].begin()) {
-    Serial.println(F("Failed to boot VL53L0X"));
-    Serial.println((int)id);
-    while(1);
-  }*/
-  while (!tofs_talk_through[id].begin()) {
-    Serial.println(F("Failed to boot VL53L0X"));
-    Serial.println("Adafruit VL53L0X XShut set Low to Force HW Reset");
-    digitalWrite(xshut.id, LOW);
-    delay(100);
-    digitalWrite(xshut.id, HIGH);
-    Serial.println("Adafruit VL53L0X XShut set high to Allow Boot");
-    delay(5000);
-  }
-  tofs_talk_through[id].setAddress(0x30 + id);
-  tube->servo.hard.attach(tube->servo.pin.id);
-  tube->servo.hard.write(minServo);
-  delay(5000);
-  Serial.println("Setuping downards");
-  float h = mesureHeight(tofs_talk_through[id], tube);
-  Serial.println(h);
-  int angle = minServo;
-  while (h > maxHeight) {
-    ++angle;
-    tube->servo.hard.write(angle);
-    delay(300);
-    h = mesureHeight(tofs_talk_through[id], tube);
-  }
-  angle -= 5;
-  tube->servo.last_angle = angle;
-  tube->servo.hard.write(tube->servo.last_angle);
-  tube->servo.equilibrium = angle;
-  Serial.println("Done.");
-}
-
-
-
-void setup_all_tubes() {
-  const struct pin all_xshuts[6] = {
-      {.id = 17, .is_extender = 0},
-      {.id = 2, .is_extender = 1},
-      {.id = 3, .is_extender = 1},
-      {.id = 7, .is_extender = 1},
-      {.id = 5, .is_extender = 1},
-      {.id = 1, .is_extender = 1}
-  };
-  const struct pin all_gpios[6] = {
-      {.id = 5, .is_extender = 0},
-      {.id = 18, .is_extender = 0},
-      {.id = 19, .is_extender = 0},
-      {.id = -1, .is_extender = 1},
-      {.id = 6, .is_extender = 1},
-      {.id = 0, .is_extender = 1}
-  };
-  const struct pin all_servos[6] = {
-      {.id = 32, .is_extender = 0},
-      {.id = 33, .is_extender = 0},
-      {.id = 23, .is_extender = 0},
-      {.id = 16, .is_extender = 0},
-      {.id = 14, .is_extender = 0},
-      {.id = 2, .is_extender = 0}
-  };
 
+// Tube configuration
+void setup_all_tofs() {
   // Reset all TOFs by turning them up and low
   for (int i = 0; i < TUBES_COUNT; ++i) {
     changePinMode(all_xshuts[i], OUTPUT);
     change_pin(all_xshuts[i], LOW);
   }
-  delay(1000);
-  /*
-  for (int i = 0; i < TUBES_COUNT; ++i) {
+
+  for (int i = 0; i<TUBES_COUNT; i++){
+    Serial.printf("\n\nInitialiazing tube #%d\n", i);
     change_pin(all_xshuts[i], HIGH);
+    Serial.println("Beginning boot sequence VL53L0X");
+    all_tubes[i].tof.hard.setTimeout(500);
+    while (!all_tubes[i].tof.hard.init()) {
+      Serial.println(F("Failed to boot VL53L0X"));
+      while(1);
+    }
+    all_tubes[i].tof.hard.setAddress(0x31 + i);
+    all_tubes[i].tof.hard.startContinuous();
+    all_tubes[i].servo.pin = all_servos[i];
+    
   }
-  delay(10);
-  for (int i = 0; i < TUBES_COUNT; ++i) {
-    change_pin(all_xshuts[i], LOW);
-  }
-  */
-
-  for (int i = 0; i < TUBES_COUNT; ++i) {
-    init_tube(all_tubes, i, all_servos[i], all_gpios[i], all_xshuts[i]);
-  }
-
+  Serial.println("Done setuping TOFs");
 }
 
+void calibrate_tube(int i){
+  Serial.printf("Calibrating tube %d.\n", i);
+  all_tubes[i].servo.hard.attach(all_tubes[i].servo.pin.id);
+  all_tubes[i].servo.hard.write(minServo);
+  delay(5000);
+  Serial.println("Setuping downards");
+  uint16_t h = mesureHeight(i);
+  int angle = minServo;
+  while (h > maxHeight) {
+    Serial.printf("%dmm %d°\n",h, angle);
+    ++angle;
+    all_tubes[i].servo.hard.write(angle);
+    delay(300);
+    h = mesureHeight(i);
+  }
+  angle -= 5;
+  all_tubes[i].servo.last_angle = angle;
+  all_tubes[i].servo.hard.write(all_tubes[i].servo.last_angle);
+  all_tubes[i].servo.equilibrium = angle;
+  Serial.printf("Tube %d calibrated.\n", i);
+}
 
+// Helper functions
+auto heightRatio(uint16_t h){
+  float height = h;
+  return max(min(1.0f, (height - minHeight) / (maxHeight-minHeight)), 0.0f);
+}
+
+int servoPosition(float ratio){
+  return (ratio * maxServo) + ((1-ratio) * minServo);
+}
 
 void setup() {
   Serial.begin(115200); // Starts the serial communication
@@ -333,6 +297,10 @@ void setup() {
     delay(1);
   }
 
+  Wire.begin();
+
+scan_I2c();
+
   if (!pcf8574.begin(0x38, &Wire)) {
     Serial.println("Couldn't find PCF8574");
     while (1);
@@ -341,70 +309,48 @@ void setup() {
     pcf8574.pinMode(p, OUTPUT);
   }
   
-  Serial.println("PCF8574 seemks OK");
-
+  Serial.println("PCF8574 is OK");
   wifisetup();
+  setup_all_tofs();
+  for (int i = 0; i<TUBES_COUNT; i++){
+    calibrate_tube(i);  
+  }
+  
 
-  //terminal.setup(&Serial);
-
-  setup_all_tubes();
-
-  Serial.println("Done setuping have funning");
 }
 
 
-auto heightRatio(float height){
-  return max(min(1.0f, (height - minHeight) / (maxHeight-minHeight)), 0.0f);
-}
-
-int servoPosition(float ratio){
-  return (ratio * maxServo) + ((1-ratio) * minServo);
-}
-
 void loop() {
-  //terminal.update();
-  for (int i = 0; i < TUBES_COUNT; i++) {
-    struct tube *tube = all_tubes + i;
-    /*
-    Serial.print("Tube ");
-    Serial.println(tube->id);
-    */
-    tube->height = mesureHeight(tofs_talk_through[tube->id], tube);
-    /*
-    Serial.print("Measure = ");
-    Serial.println(tube->height);
-    */
-    int angle = 0;
-    float diff = tube->height - tube->target;
-    if (abs(diff) > 5.0f) {
-      angle = tube->servo.equilibrium + (diff * 1 / (7));
-    } else if (abs(diff) > 2.0f) {
-      angle = tube->servo.equilibrium + (diff * 1 / (7));
-    } else {
-      angle = tube->servo.equilibrium;
-    }
-    /*
-    Serial.print("angle = ");
-    Serial.print(angle);
-    Serial.print("; equilibrium = ");
-    Serial.println(tube->servo.equilibrium);
-    */
-    tube->servo.last_angle = min(max(angle, minServo), maxServo);
-   /* Serial.print("Diff = ");
-    Serial.print(diff);
-    Serial.print("; Requested = ");
-    Serial.print(tube->target);
-    Serial.print("; Angle = ");
-    Serial.println(tube->servo.last_angle);
-    */
-    tube->servo.hard.write(tube->servo.last_angle);
-  }
-
   for (int i = 0; i<TUBES_COUNT; i++){
-    Serial.print((&all_tubes[i])->height);
-    Serial.print("\t");
+    all_tubes[i].height = mesureHeight(i);
+    Serial.printf("%d", all_tubes[i].height);
+
+    int angle = 0;
+    int diff = all_tubes[i].height - all_tubes[i].target;
+
+
+    if (abs(diff) > 5.0f) {
+      angle = all_tubes[i].servo.equilibrium + (diff * 1 / (7));
+    } else if (abs(diff) > 2.0f) {
+      angle = all_tubes[i].servo.equilibrium + (diff * 1 / (7));
+    } else {
+      angle = all_tubes[i].servo.equilibrium;
+    }
+
+    Serial.printf("%d - %d = %d => %d\n", all_tubes[i].height, all_tubes[i].target, diff, angle);
+
+    
+    all_tubes[i].servo.last_angle = min(max(angle, minServo), maxServo);
+    all_tubes[i].servo.hard.write(all_tubes[i].servo.last_angle);
+
+
   }
   Serial.println();
+  
+  //Serial.printf("%d %d %d %d %d %d \n", val0, val1, val2, val3, val4, val5);
+ // Serial.printf("%d %d %d \n", val3, val4, val5);
+  //Serial.printf("%d %d \n", val0, val1);
 
-  delay(1);
+  delay(5);
 }
+