/*
Name: Teensy_Hex_VL53L0X_Demo_V3.ino
Created: 7/22/2020 11:31:10 AM
Author: FRANKNEWXPS15\Frank
*/
/*
Name: Teensy_Hex_V53L0X.ino
Created: 7/16/2020 2:56:35 PM
Author: FRANKNEWXPS15\Frank
Notes:
07/18/20: I discovered that only 3 VL53L0X units can share the I2C bus. The GY530
modules have internal 10K pullups on the I2C lines, and the 4th one on the
bus reduces the combined pullup value too much.
Instead, I put the right-hand array of 3 on Wire1 and the left-hand array
of 3 on Wire2.
*/
/*
Name: Teensy_Triple_VL53L0X_V2.ino
Created: 5/23/2020 1:16:52 PM
Author: FRANKNEWXPS15\Frank
*/
/* This code copied from the following post on the Adafruit forum:
https://forums.adafruit.com/viewtopic.php?f=19&t=164589&p=808693&hilit=vl53l0x#p808693
And adapted to use it on the Wire1 I2C bus on a Teensy 3.5
*/
#include <Wire.h>
#include "Adafruit_VL53L0X.h"
Adafruit_VL53L0X lidar_RF = Adafruit_VL53L0X(); //Right-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_RC = Adafruit_VL53L0X(); //Right-center LIDAR
Adafruit_VL53L0X lidar_RR = Adafruit_VL53L0X(); //Right-rear LIDAR (angled 30 deg rearward)
//07/16/20 added three left-side sensors
Adafruit_VL53L0X lidar_LF = Adafruit_VL53L0X(); //Left-front LIDAR (angled 30 deg forward)
Adafruit_VL53L0X lidar_LC = Adafruit_VL53L0X(); //Left-center LIDAR
Adafruit_VL53L0X lidar_LR = Adafruit_VL53L0X(); //left-rear LIDAR (angled 30 deg rearward)
//XSHUT required for I2C address init
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;
const int LF_XSHUT_PIN = 5;
const int LC_XSHUT_PIN = 6;
const int LR_XSHUT_PIN = 7;
const int MAX_LIDAR_RANGE_MM = 1000; //make it obvious when nearest object is out of range
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;
void setup()
{
Serial.begin(115200); //Teensy ignores rate parameter
pinMode(RF_XSHUT_PIN, OUTPUT);
pinMode(RC_XSHUT_PIN, OUTPUT);
pinMode(RR_XSHUT_PIN, OUTPUT);
pinMode(LF_XSHUT_PIN, OUTPUT);
pinMode(LC_XSHUT_PIN, OUTPUT);
pinMode(LR_XSHUT_PIN, OUTPUT);
// wait until serial port opens for native USB devices
while (!Serial)
{
delay(1);
}
delay(3000); //05/28/20 - this delay is necessary AFTER Serial instantiation!
Serial.printf("Teensy Hex VL53L0X Demo\n");
//initialize all six LIDAR sensors
// 1. Reset all sensors by setting all of their XSHUT pins low for delay(10), then set
// all XSHUT high to bring out of reset
//right side
digitalWrite(RF_XSHUT_PIN, LOW);
delay(10);
digitalWrite(RC_XSHUT_PIN, LOW);
delay(10);
digitalWrite(RR_XSHUT_PIN, LOW);
delay(10);
//right side
digitalWrite(RF_XSHUT_PIN, HIGH);
digitalWrite(RC_XSHUT_PIN, HIGH);
digitalWrite(RR_XSHUT_PIN, HIGH);
delay(1000);
// 2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
// shutdown by pulling XSHUT pins low
//right side
digitalWrite(RC_XSHUT_PIN, LOW);
digitalWrite(RR_XSHUT_PIN, LOW);
// 4. Initialize sensor #1
if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, false, &Wire1))
//if (!lidar_RF.begin(VL53L0X_I2C_ADDR + 1, true, &Wire1))
{
Serial.println(F("Failed to boot lidar_RF"));
while (1);
}
delay(100);
// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
// you set the first sensor to
digitalWrite(RC_XSHUT_PIN, HIGH);
if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, false, &Wire1))
//if (!lidar_RC.begin(VL53L0X_I2C_ADDR + 2, true, &Wire1))
{
Serial.println(F("Failed to boot lidar_RC"));
while (1);
}
//delay(100);
// 7. Repeat for each sensor, turning each one on, setting a unique address.
digitalWrite(RR_XSHUT_PIN, HIGH);
if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, false, &Wire1))
//if (!lidar_RR.begin(VL53L0X_I2C_ADDR + 3, true, &Wire1))
{
Serial.println(F("Failed to boot lidar_RR"));
while (1);
}
//delay(100);
//07/21/20 now go back and verify that all I2C addresses are unique
Serial.printf("Scanning Wire1...");
int nDevices = 0;
for (byte address = 1; address < 127; ++address) {
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
//Wire.beginTransmission(address);
Wire1.beginTransmission(address);
byte error = Wire1.endTransmission();
if (error == 0) {
Serial.print("I2C device found at address 0x");
if (address < 16) {
Serial.print("0");
}
Serial.print(address, HEX);
Serial.println(" !");
++nDevices;
}
else if (error == 4) {
Serial.print("Unknown error at address 0x");
if (address < 16) {
Serial.print("0");
}
Serial.println(address, HEX);
}
}
if (nDevices == 0) {
Serial.println("No I2C devices found\n");
}
else {
Serial.println("done\n");
}
delay(2000);
//left side
digitalWrite(LF_XSHUT_PIN, LOW);
delay(10);
digitalWrite(LC_XSHUT_PIN, LOW);
delay(10);
digitalWrite(LR_XSHUT_PIN, LOW);
delay(10);
digitalWrite(LF_XSHUT_PIN, HIGH);
digitalWrite(LC_XSHUT_PIN, HIGH);
digitalWrite(LR_XSHUT_PIN, HIGH);
digitalWrite(LC_XSHUT_PIN, LOW);
digitalWrite(LR_XSHUT_PIN, LOW);
//07/15/20 do same thing for left side sensors
//2. Keep sensor #1 awake by keeping XSHUT pin high, and put all other sensors into
// shutdown by pulling XSHUT pins low
// 4. Initialize sensor #1
if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, false, &Wire2))
// if (!lidar_LF.begin(VL53L0X_I2C_ADDR + 4, true, &Wire2))
{
Serial.println(F("Failed to boot lidar_LF"));
while (1);
}
//delay(100);
// 5. Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
// 6. Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever
// you set the first sensor to
digitalWrite(LC_XSHUT_PIN, HIGH);
if (!lidar_LC.begin(VL53L0X_I2C_ADDR + 5, false, &Wire2))
{
Serial.println(F("Failed to boot lidar_RC"));
while (1);
}
//delay(100);
// 7. Repeat for each sensor, turning each one on, setting a unique address.
digitalWrite(LR_XSHUT_PIN, HIGH);
if (!lidar_LR.begin(VL53L0X_I2C_ADDR + 6, false, &Wire2))
{
Serial.println(F("Failed to boot lidar_LR"));
while (1);
}
//delay(100);
Serial.printf("\nInit complete\n \nVL53L0X API Simple Ranging example\n \n");
Serial.printf("Scanning Wire2...");
nDevices = 0;
for (byte address = 1; address < 127; ++address) {
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
//Wire.beginTransmission(address);
Wire2.beginTransmission(address);
byte error = Wire2.endTransmission();
if (error == 0) {
Serial.print("I2C device found at address 0x");
if (address < 16) {
Serial.print("0");
}
Serial.print(address, HEX);
Serial.println(" !");
++nDevices;
}
else if (error == 4) {
Serial.print("Unknown error at address 0x");
if (address < 16) {
Serial.print("0");
}
Serial.println(address, HEX);
}
}
if (nDevices == 0) {
Serial.println("No I2C devices found\n");
}
else {
Serial.println("done\n");
}
delay(2000);
Serial.printf("Msec\tFront\tCenter\tRear\tSteer\n");
}
void loop()
{
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;
VL53L0X_RangingMeasurementData_t measure3;
VL53L0X_RangingMeasurementData_t measure4;
VL53L0X_RangingMeasurementData_t measure5;
VL53L0X_RangingMeasurementData_t measure6;
lidar_RF.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
//lidar_RF.rangingTest(&measure1, true); // pass in 'true' to get debug data printout!
if (measure1.RangeStatus != 4) // phase failures have incorrect data
{
RF_Dist_mm = measure1.RangeMilliMeter;
if (RF_Dist_mm > MAX_LIDAR_RANGE_MM) RF_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else RF_Dist_mm = 0;
delay(100);
lidar_RC.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
//lidar_RC.rangingTest(&measure2, true); // pass in 'true' to get debug data printout!
if (measure2.RangeStatus != 4) // phase failures have incorrect data
{
RC_Dist_mm = measure2.RangeMilliMeter;
if (RC_Dist_mm > MAX_LIDAR_RANGE_MM) RC_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else RC_Dist_mm = 0;
delay(100);
lidar_RR.rangingTest(&measure3, false); // pass in 'true' to get debug data printout!
//lidar_RR.rangingTest(&measure3, true); // pass in 'true' to get debug data printout!
if (measure3.RangeStatus != 4) // phase failures have incorrect data
{
RR_Dist_mm = measure3.RangeMilliMeter;
if (RR_Dist_mm > MAX_LIDAR_RANGE_MM) RR_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else RR_Dist_mm = 0;
//07/16/20 added left side
lidar_LF.rangingTest(&measure4, false); // pass in 'true' to get debug data printout!
//lidar_LF.rangingTest(&measure4, true); // pass in 'true' to get debug data printout!
if (measure4.RangeStatus != 4) // phase failures have incorrect data
{
LF_Dist_mm = measure4.RangeMilliMeter;
if (LF_Dist_mm > MAX_LIDAR_RANGE_MM) LF_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else LF_Dist_mm = 0;
delay(100);
lidar_LC.rangingTest(&measure5, false); // pass in 'true' to get debug data printout!
//lidar_LC.rangingTest(&measure5, true); // pass in 'true' to get debug data printout!
if (measure5.RangeStatus != 4) // phase failures have incorrect data
{
LC_Dist_mm = measure5.RangeMilliMeter;
if (LC_Dist_mm > MAX_LIDAR_RANGE_MM) LC_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else LC_Dist_mm = 0;
delay(100);
lidar_LR.rangingTest(&measure6, false); // pass in 'true' to get debug data printout!
//lidar_LR.rangingTest(&measure6, true); // pass in 'true' to get debug data printout!
if (measure6.RangeStatus != 4) // phase failures have incorrect data
{
LR_Dist_mm = measure6.RangeMilliMeter;
if (LR_Dist_mm > MAX_LIDAR_RANGE_MM) LR_Dist_mm = MAX_LIDAR_RANGE_MM;
}
else RR_Dist_mm = 0;
Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\n", millis(), RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
LF_Dist_mm, LC_Dist_mm, LR_Dist_mm);
delay(250);
}