Here is a good starting point, however, it might need a polynomial regression.
#include <Wire.h>
#include <VL53L0X.h>
#include <Preferences.h>
VL53L0X sensor;
Preferences laserPrefs;
const int numMeasurements = 100; // Number of measurements to average
const int numCalPoints = 7; // Number of calibration points
uint16_t measuredDistances[numCalPoints];
uint16_t actualDistances[numCalPoints] = {10, 100, 300, 600, 900, 1000, 1050}; // Replace with your actual distances in mm
// Calibration parameters
float offset = 0.0;
float scale = 1.0;
void setup()
{
Serial.begin(9600);
delay(500);
Wire.begin();
// Initialize Preferences for laser calibration data (read-write)
laserPrefs.begin("laser_calibration", false); // Initialize for writing
if (!laserPrefs.isKey("offset")) {
Serial.println("No laser calibration data found. Please calibrate and store offsets.");
// Perform laser sensor calibration and store offsets if necessary
} else {
Serial.println("Laser calibration data loaded.");
// Load laser calibration offsets if they exist
}
sensor.setTimeout(500);
if (!sensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1) {}
}
// Start continuous back-to-back mode (take readings as
// fast as possible). To use continuous timed mode
// instead, provide a desired inter-measurement period in
// ms (e.g. sensor.startContinuous(100)).
sensor.startContinuous();
// Perform calibration if no stored data found
for (int i = 0; i < numCalPoints; i++) {
Serial.print("Place the sensor at ");
Serial.print(actualDistances[i]);
Serial.println(" mm");
delay(5000); // Wait for user to place the sensor
measuredDistances[i] = measureDistance();
Serial.print("Measured distance: ");
Serial.println(measuredDistances[i]);
// Prompt for user confirmation of measured distance
if (!confirmDistance(actualDistances[i], measuredDistances[i])) {
Serial.println("Measurement rejected. Please adjust sensor position and retry.");
i--; // Retry measurement for the same distance
} else {
Serial.println("Measurement accepted.");
}
}
// Calculate calibration parameters
offset = calculateOffset();
scale = calculateScale();
// Store calibration data
laserPrefs.putFloat("offset", offset);
laserPrefs.putFloat("scale", scale);
Serial.println("Calibration data stored in laserPrefs");
Serial.print("Calibration offset: ");
Serial.println(offset);
Serial.print("Calibration scale: ");
Serial.println(scale);
Serial.println("Entering main loop...");
}
void loop()
{
int tofDist = sensor.readRangeContinuousMillimeters();
if(tofDist < 2000 && tofDist != 8191){
float calibratedDist = tofDist * scale + offset;
Serial.print("Raw Distance: ");
Serial.print(tofDist);
Serial.print(" mm, Calibrated Distance: ");
Serial.print(calibratedDist);
Serial.println(" mm");
delay(1000); // Adjust delay as necessary
}
//if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
}
uint16_t measureDistance() {
uint32_t sum = 0; // Use uint32_t to avoid overflow if the sum gets large
int validCount = 0;
for (int i = 0; i < numMeasurements; i++) {
uint16_t distance = sensor.readRangeContinuousMillimeters();
if (distance < 4000) { // Check if the distance is valid (assuming valid distances are < 4000 mm)
sum += distance;
validCount++;
Serial.print("VM: ");
Serial.print(validCount);
Serial.print(" ");
Serial.println(distance);
} else {
// Handle invalid measurements if necessary
Serial.print("VM: ");
Serial.print(validCount);
Serial.print(" Invalid measurement: ");
Serial.println(distance);
}
delay(50); // Small delay between measurements
}
Serial.print("SUM: ");
Serial.println(sum);
Serial.print("Valid measurements: ");
Serial.println(validCount);
if (validCount > 0) {
return sum / validCount; // Return the average of valid measurements
} else {
return 0; // Return 0 or handle this case appropriately if no valid measurements were recorded
}
}
float calculateOffset() {
float sum = 0;
for (int i = 0; i < numCalPoints; i++) {
sum += (actualDistances[i] - measuredDistances[i]);
}
return sum / numCalPoints;
}
float calculateScale() {
float sum = 0;
for (int i = 0; i < numCalPoints; i++) {
sum += (actualDistances[i] / measuredDistances[i]);
}
return sum / numCalPoints;
}
bool confirmDistance(uint16_t actual, uint16_t measured) {
Serial.print("Measured distance is ");
Serial.print(measured);
Serial.print(" mm. Is this correct for ");
Serial.print(actual);
Serial.println(" mm? (y/n)");
while (true) {
if (Serial.available() > 0) {
char response = Serial.read();
if (response == 'y' || response == 'Y') {
return true;
} else if (response == 'n' || response == 'N') {
return false;
}
}
delay(100); // Wait for user input
}
}
No comments:
Post a Comment