From 0e0781360c9b660ce7548cad441bdb65674d7dc0 Mon Sep 17 00:00:00 2001
From: Alessevan <44125445+Alessevan@users.noreply.github.com>
Date: Mon, 5 May 2025 09:12:57 +0200
Subject: [PATCH] feat: generic

---
 prog/prog.ino | 128 +++++++++++++++++++++++++++++++-------------------
 1 file changed, 80 insertions(+), 48 deletions(-)

diff --git a/prog/prog.ino b/prog/prog.ino
index b4254f2..d8c2005 100644
--- a/prog/prog.ino
+++ b/prog/prog.ino
@@ -1,7 +1,22 @@
 #include <ESP32Servo.h>
 #include "Adafruit_VL53L0X.h"
 
-Adafruit_VL53L0X lox = Adafruit_VL53L0X();
+struct tube {
+  float height;
+  float target;
+  struct {
+    Servo hard;
+    int pin;
+    int last_angle;
+    int equilibrium;
+  } servo;
+  struct {
+    Adafruit_VL53L0X hard;
+    char gpio;
+    char xshut;
+  } tof;
+  int id;
+};
 
 static const int servoPin = 13;
 
@@ -90,7 +105,7 @@ String processor(const String& var){
   return String();
 }
 
-auto mesureHeight(){
+auto mesureHeight(Adafruit_VL53L0X lox){
   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
@@ -149,6 +164,46 @@ void wifisetup() {
   server.begin();
 }
 
+struct tube all_tubes[6] = {0};
+
+
+void init_tube(struct tube *tube, char id, char servo_pin, char gpio, char xshut) {
+  Serial.print(" Initialiazing tube #");
+  Serial.println(id);
+  tube->id = id;
+  tube->servo.pin = servo_pin;
+  tube->tof.gpio = gpio;
+  tube->tof.xshut = xshut;
+  tube->height = 250.0;
+  tube->target = 250.0;
+
+  tube->tof.hard = Adafruit_VL53L0X();
+  if (!tube->tof.hard.begin()) {
+    Serial.println(F("Failed to boot VL53L0X"));
+    while(1);
+  }
+  tube->servo.hard.attach(tube->servo.pin);
+  tube->servo.hard.write(minServo);
+  delay(5000);
+  Serial.println("Setuping downards");
+  float h = mesureHeight(tube->tof.hard);
+  int angle = minServo;
+  while (h > maxHeight) {
+    ++angle;
+    tube->servo.hard.write(angle);
+    delay(100);
+    h = mesureHeight(tube->tof.hard);
+  }
+  angle -= 2;
+  tube->servo.hard.write(angle);
+  tube->servo.equilibrium = angle;
+  Serial.println("Done.");
+}
+
+void setup_all_tubes() {
+  init_tube(all_tubes, 1, 32, 5, 17);
+}
+
 void setup() {
   Serial.begin(115200); // Starts the serial communication
   while (! Serial) {
@@ -157,34 +212,7 @@ void setup() {
 
   wifisetup();
 
-  servo1.attach(servoPin);
-
-  Serial.println("Adafruit VL53L0X test");
-  if (!lox.begin()) {
-    Serial.println(F("Failed to boot VL53L0X"));
-    while(1);
-  }
-  Serial.println(F("VL53L0X API Simple Ranging example\n\n")); 
-
-  Serial.println("Setuping");
-  servo1.write(minServo);
-  delay(5000);
-  float h = mesureHeight();
-  if (h < maxHeight) {
-    Serial.println("CHANGE FAN SPEED");
-    while (1);
-  }
-  Serial.println("Setuping for downwards");
-  int angle = minServo;
-  while (h > maxHeight) {
-    ++angle;
-    servo1.write(angle);
-    delay(100);
-    h = mesureHeight();
-  }
-  angle -= 2;
-  servo1.write(angle);
-  equilibrium = angle;
+  setup_all_tubes();
   Serial.println("Done setuping have funning");
 }
 
@@ -199,25 +227,29 @@ int servoPosition(float ratio){
 
 void loop() {
    
-  float h = mesureHeight();
-  Serial.print("Measure = ");
-  Serial.println(h);
-  int angle = 0;
-  if (abs(h - requestedValue) > 5.0f) {
-    Serial.println("HEYEYEYEYEYEYEY");
-    angle = equilibrium + ((h - requestedValue) * 1 / (7));
-  } else if (abs(h - requestedValue) > 2.0f) {
-    angle = equilibrium + ((h - requestedValue) * 1 / (7));
-  } else {
-    error = 0;
-    angle = equilibrium;
+  for (int i = 0; i < 1; i++) {
+    struct tube tube = all_tubes[i];
+    tube.height = mesureHeight(tube.tof.hard);
+    Serial.print("Measure = ");
+    Serial.println(tube.height);
+    int angle = 0;
+    float diff = tube.height - tube.target;
+    if (abs(diff) > 5.0f) {
+      Serial.println("HEYEYEYEYEYEYEY");
+      angle = tube.servo.equilibrium + (diff * 1 / (7));
+    } else if (abs(diff) > 2.0f) {
+      angle = tube.servo.equilibrium + (diff * 1 / (7));
+    } else {
+      error = 0;
+      angle = tube.servo.equilibrium;
+    }
+    Serial.print("angle = ");
+    Serial.print(angle);
+    Serial.print("; equilibrium = ");
+    Serial.println(tube.servo.equilibrium);
+    tube.servo.last_angle = angle;
+    tube.servo.hard.write(min(max(angle, minServo), maxServo));
   }
-  error += requestedValue - h;
-  Serial.print("angle = ");
-  Serial.print(angle);
-  Serial.print("; equilibrium = ");
-  Serial.println(equilibrium);
-  servo1.write(min(max(angle, minServo), maxServo));
 
   delay(1);
 }
\ No newline at end of file