From d23392705ce1cc163d09bddba26095c15fa48e24 Mon Sep 17 00:00:00 2001
From: Pierre Tellier <piair338@gmail.com>
Date: Thu, 10 Apr 2025 14:39:28 +0200
Subject: [PATCH] added current version

---
 prog/prog.ino | 81 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 81 insertions(+)
 create mode 100644 prog/prog.ino

diff --git a/prog/prog.ino b/prog/prog.ino
new file mode 100644
index 0000000..501ec39
--- /dev/null
+++ b/prog/prog.ino
@@ -0,0 +1,81 @@
+#include <ESP32Servo.h>
+#include "Adafruit_VL53L0X.h"
+
+Adafruit_VL53L0X lox = Adafruit_VL53L0X();
+
+static const int servoPin = 13;
+
+Servo servo1;
+const int SDL = 22;
+const int SDA2 = 21; // useless in the code
+
+const int minHeight = 5;
+const int maxHeight = 600;
+
+int minServo = 0;
+int maxServo = 90;
+
+float curHeight = 0; 
+
+void setup() {
+  Serial.begin(115200); // Starts the serial communication
+  while (! Serial) {
+    delay(1);
+  }
+  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")); 
+}
+
+
+
+auto mesureHeight(){
+  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){
+      curHeight = height;
+      return height;
+    } else {
+      Serial.println("Incoherent measurement. Returning previous one.");
+      return curHeight;
+    }
+  }
+  else {
+    Serial.println(" out of range ");
+    return curHeight;
+  }
+}
+
+auto heightRatio(float height){
+  return min(1.0f, (height - minHeight) / (maxHeight-minHeight));
+}
+
+int servoPosition(float ratio){
+  return (ratio * maxServo) + ((1-ratio) * minServo);
+}
+
+void loop() {
+  
+   
+  float h = mesureHeight();
+  float ratio = heightRatio(h);
+  int servoRot = servoPosition(ratio);
+  
+  /*
+  Serial.print("h: ");
+  Serial.print(h);
+  Serial.print(" ratio: ");
+  Serial.print(ratio);
+  Serial.print("rotation: ");
+  Serial.println(servoRot);
+*/
+  servo1.write(servoRot);
+  delay(1);
+}
\ No newline at end of file