81 lines
1.7 KiB
C++
81 lines
1.7 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 = 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);
|
|
} |