Das Programm testet den auf einem Servo befindlichen Abstandssensor VL53L0X und gibt ein Diagramm aus, das den Abstand über dem Winkel aufträgt.
Das Video dazu findet ihr im Weihnachtskalender.
Autor: Christian
/* BEERLECADA: This example shows how to use continuous mode to take
range measurements with the VL53L0X. It is based on
vl53l0x_ContinuousRanging_Example.c from the VL53L0X API.
The obstacle has to be less than 250 mm away and the Serial monitor
shows a graph of the distances over the angle.
Programmer: Christian Rempel, provided against a beer (red wine) licence
The range readings are in units of mm. */
/*
-SCL=A5
-SDA=A4 for PCA9685 and VL53L0/1XV2
servo on PCA9685 place 12
*/
#include <Wire.h>
#include <VL53L0X.h>
#include <Adafruit_PWMServoDriver.h>
#define startpos 64
#define endpos 145
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
#define SERVOMIN 80 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 530 // This is the 'maximum' pulse length count (out of 4096)
//Servo myservo; // erstellt ein Servo-Objekt, um einen Servomotor zu steuern
VL53L0X sensor;
byte dist[180];
int angle;
long steptimer, printtimer;
long loopcounter;
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
void setup()
{
Serial.begin(9600);
Wire.begin();
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
sensor.setTimeout(500);
if (!sensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
delay(1000);
}
// 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(20);
angle = startpos - 1;
//myservo.write(angle);
delay(500);
steptimer = millis();
printtimer = millis();
}
void loop()
{ int distance;
loopcounter++;
if (millis() - steptimer > 1) {
angle -= startpos;
angle++;
angle %= endpos + 1 - startpos;
angle += startpos;
pwm.setPWM(12, 0, SERVOMIN + (int)(angle * 2.5));
//Serial.println(angle);
steptimer = millis();
distance = sensor.readRangeContinuousMillimeters();
if (distance > 255) dist[angle] = 255;
else dist[angle] = distance;
}
if (sensor.timeoutOccurred()) {
Serial.print(" TIMEOUT");
}
if (angle == endpos) {
for (int i = startpos; i <= endpos; i+=2) {
//Serial.print(i);
distance = (int)((dist[i] + dist[i + 1]) / 8);
for ( int ii = 0; distance > ii; ii++) {
Serial.print("|");
}
//Serial.println((String)i + " = i distance = " + dist[i]);
//Serial.println(dist[i]);
Serial.println("");
}
Serial.println(" loopcounter = " + (String)loopcounter);
loopcounter = 0;
pwm.setPWM(12, 0, SERVOMIN + (int)(startpos * 2.5));
delay(500);
}
}