less i2c errors

prog/prog.ino

@@ -2,6 +2,11 @@
 #include <Wire.h>
 #include "Adafruit_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
@@ -37,10 +42,12 @@ TERMINAL_COMMAND(terminal, ping, "Print pong followoing by the received argument
     }
     terminal.println("");
   }
+  TERMINAL_ADD_DEFAULT_COMMANDS(terminal) // help, params, load_config, reset_config, echo ...
+
+  terminal_n::Terminal terminal("Pingpong flottant.\n\r\n\r");
+
+*/
 
-#include <WiFi.h>
-#include <AsyncTCP.h>
-#include <ESPAsyncWebServer.h>
 
 struct pin {
     int id;
@@ -64,6 +71,8 @@ struct tube {
   int id;
 };
 
+
+const int TUBES_COUNT = 6;
 struct tube all_tubes[6] = {0};
 
 const float minHeight = 50;
@@ -118,6 +127,7 @@ function sendSliderValue(id, element) {
   </html>
 )rawliteral";
 
+
 // Replaces placeholder with button section in your web page
 String processor(const String& var){
   //Serial.println(var);
@@ -135,6 +145,7 @@ String processor(const String& var){
   return String();
 }
 
+
 auto mesureHeight(Adafruit_VL53L0X lox, struct tube *tube){
   VL53L0X_RangingMeasurementData_t measure;
   lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
@@ -149,11 +160,12 @@ auto mesureHeight(Adafruit_VL53L0X lox, struct tube *tube){
     }
   }
   else {
-    Serial.println(" out of range ");
+    //Serial.println(" out of range ");
     return tube->height;
   }
 }
 
+
 void wifisetup() {
   Serial.print("Setting AP (Access Point)…");
   WiFi.softAP(ssid);
@@ -202,8 +214,18 @@ void change_pin(struct pin pin, int 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);
@@ -213,21 +235,32 @@ void init_tube(struct tube *tube, char id, struct pin servo_pin, struct pin 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);
+  //change_pin(servo_pin, HIGH);
-
   Serial.println("Beginning boot sequence VL53L0X");
-  if (!tofs_talk_through[id].begin(0x30 + id)) {
+  /*
+  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;
@@ -242,22 +275,22 @@ void init_tube(struct tube *tube, char id, struct pin servo_pin, struct pin gpio
   Serial.println("Done.");
 }
 
-const int ACTIVES = 1;
+
 
 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 = 5, .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 = 4, .is_extender = 1},
+      {.id = -1, .is_extender = 1},
       {.id = 6, .is_extender = 1},
       {.id = 0, .is_extender = 1}
   };
@@ -269,25 +302,29 @@ void setup_all_tubes() {
       {.id = 14, .is_extender = 0},
       {.id = 2, .is_extender = 0}
   };
-  for (int i = 0; i < ACTIVES; ++i) {
+
+  // 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(10);
+  delay(1000);
-  for (int i = 0; i < ACTIVES; ++i) {
+  /*
+  for (int i = 0; i < TUBES_COUNT; ++i) {
     change_pin(all_xshuts[i], HIGH);
   }
   delay(10);
-  for (int i = 0; i < ACTIVES; ++i) {
+  for (int i = 0; i < TUBES_COUNT; ++i) {
     change_pin(all_xshuts[i], LOW);
   }
-  for (int i = 0; i < ACTIVES; ++i) {
+  */
+
+  for (int i = 0; i < TUBES_COUNT; ++i) {
     init_tube(all_tubes, i, all_servos[i], all_gpios[i], all_xshuts[i]);
   }
+
 }
 
-TERMINAL_ADD_DEFAULT_COMMANDS(terminal) // help, params, load_config, reset_config, echo ...
-
-terminal_n::Terminal terminal("Pingpong flottant.\n\r\n\r");
 
 
 void setup() {
@@ -308,7 +345,7 @@ void setup() {
 
   wifisetup();
 
-  terminal.setup(&Serial);
+  //terminal.setup(&Serial);
 
   setup_all_tubes();
 
@@ -325,8 +362,8 @@ int servoPosition(float ratio){
 }
 
 void loop() {
-  terminal.update();
+  //terminal.update();
-  for (int i = 0; i < ACTIVES; i++) {
+  for (int i = 0; i < TUBES_COUNT; i++) {
     struct tube *tube = all_tubes + i;
     /*
     Serial.print("Tube ");
@@ -359,8 +396,15 @@ void loop() {
     Serial.print(tube->target);
     Serial.print("; Angle = ");
     Serial.println(tube->servo.last_angle);
-    */tube->servo.hard.write(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");
+  }
+  Serial.println();
+
   delay(1);
 }