99 lines
2.1 KiB
C++
99 lines
2.1 KiB
C++
#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 = 50;
|
|
const int maxHeight = 455;
|
|
|
|
int minServo = 30;
|
|
int maxServo = 60;
|
|
int lastAngle = -1;
|
|
|
|
|
|
float curHeight = 50;
|
|
float targetHeight = 250;
|
|
|
|
float equilibrium = 0;
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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"));
|
|
|
|
Serial.println("Setuping");
|
|
servo1.write(minServo);
|
|
delay(2000);
|
|
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;
|
|
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() {
|
|
|
|
float h = mesureHeight();
|
|
Serial.print("Measure = ");
|
|
Serial.println(h);
|
|
if (abs(h - targetHeight) > 1.0f) {
|
|
servo1.write(equilibrium + 5);
|
|
}
|
|
|
|
delay(1);
|
|
} |