I ran across this topic while looking for solutions to my own problem with too-slow measurement responses for a wall-following robot project, and thought my testing results using a 6-element array of VL53L0X sensors might be useful for others.
I combined the code from the above posts with my setup code for multiple sensors, and recorded the distance measurements from all six sensors. The interesting this is the total loop time required to produce measurements for all six sensors was still only about 18-19 mSec. IOW (assuming I’m not blowing smoke here), the additional five sensors don’t impose any significant additional time penalty!
Here’s my code:
/*
Name: TeensyHexVL53L0XHighSpeedDemo.ino
Created: 10/1/2020 3:49:01 PM
Author: FRANKNEWXPS15\Frank
This demo program combines the code from my Teensy_VL53L0X_I2C_Slave_V2
project with the VL53L0X 'high speed' code from the Pololu support
forum post at https://forum.pololu.com/t/high-speed-with-vl53l0x/16585/3
*/
/*
Name: Teensy_VL53L0X_I2C_Slave_V2.ino
Created: 8/4/2020 7:42:22 PM
Author: FRANKNEWXPS15\Frank
This project merges KurtE's multi-wire capable version of Pololu's VL53L0X library
with my previous Teensy_VL53L0X_I2C_Slave project, which used the Adafruit VL53L0X
library. This (hopefully) will be the version uploaded to the Teensy on the 2nd
deck of Wall-E2 and which controls the left & right 3-VL53L0X sensor arrays
*/
#include <Wire.h> //using Wire.h here instead of i2c_t3.h for multiple I2C bus access
#include <VL53L0X.h>
#include "I2C_Anything.h" //local version modified to use Wire.h vs SBWire
volatile uint8_t request_type;
enum VL53L0X_REQUEST
{
VL53L0X_CENTERS_ONLY,
VL53L0X_RIGHT,
VL53L0X_LEFT,
VL53L0X_BOTH //added 08/05/20
};
//right side array
VL53L0X lidar_RF(&Wire1);
VL53L0X lidar_RC(&Wire1);
VL53L0X lidar_RR(&Wire1);
//left side array
VL53L0X lidar_LF(&Wire2);
VL53L0X lidar_LC(&Wire2);
VL53L0X lidar_LR(&Wire2);
//XSHUT required for I2C address init
//right side array
const int RF_XSHUT_PIN = 23;
const int RC_XSHUT_PIN = 22;
const int RR_XSHUT_PIN = 21;
//left side array
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
//right side array
uint16_t RF_Dist_mm;
uint16_t RC_Dist_mm;
uint16_t RR_Dist_mm;
//left side array
uint16_t LF_Dist_mm;
uint16_t LC_Dist_mm;
uint16_t LR_Dist_mm;
float RightSteeringVal;
float LeftSteeringVal;
const int SLAVE_ADDRESS = 0x20; //just a guess for now
const int DEFAULT_VL53L0X_ADDR = 0x29;
void setup()
{
Serial.begin(115200);
Wire.begin(SLAVE_ADDRESS); //set Teensy Wire0 port up as slave
Wire.onRequest(requestEvent); //ISR for I2C requests from master (Mega 2560)
Wire.onReceive(receiveEvent); //ISR for I2C data from master (Mega 2560)
Wire1.begin();
Wire2.begin();
// wait until serial port opens ... For 5 seconds max
while (!Serial && millis() < 5000);
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);
//Put all sensors in reset mode by pulling XSHUT low
digitalWrite(RF_XSHUT_PIN, LOW);
digitalWrite(RC_XSHUT_PIN, LOW);
digitalWrite(RR_XSHUT_PIN, LOW);
digitalWrite(LF_XSHUT_PIN, LOW);
digitalWrite(LC_XSHUT_PIN, LOW);
digitalWrite(LR_XSHUT_PIN, LOW);
//now bring lidar_RF only out of reset and set it's address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(RF_XSHUT_PIN, INPUT);
delay(10);
if (!lidar_RF.init())
{
Serial.println("Failed to detect and initialize lidar_RF!");
while (1) {}
}
//from Pololu forum post
lidar_RF.setMeasurementTimingBudget(20000);
lidar_RF.startContinuous();
lidar_RF.setAddress(DEFAULT_VL53L0X_ADDR + 1);
Serial.printf("lidar_RF address is 0x%x\n", lidar_RF.getAddress());
//now bring lidar_RC only out of reset, initialize it, and set its address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(RC_XSHUT_PIN, INPUT);
delay(10);
if (!lidar_RC.init())
{
Serial.println("Failed to detect and initialize lidar_RC!");
while (1) {}
}
//from Pololu forum post
lidar_RC.setMeasurementTimingBudget(20000);
lidar_RC.startContinuous();
lidar_RC.setAddress(DEFAULT_VL53L0X_ADDR + 2);
Serial.printf("lidar_RC address is 0x%x\n", lidar_RC.getAddress());
//now bring lidar_RR only out of reset, initialize it, and set its address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(RR_XSHUT_PIN, INPUT);
delay(10);
if (!lidar_RR.init())
{
Serial.println("Failed to detect and initialize lidar_RR!");
while (1) {}
}
//from Pololu forum post
lidar_RR.setMeasurementTimingBudget(20000);
lidar_RR.startContinuous();
lidar_RR.setAddress(DEFAULT_VL53L0X_ADDR + 3);
Serial.printf("lidar_RR address is 0x%x\n", lidar_RR.getAddress());
//now bring lidar_LF only out of reset, initialize it, and set its address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(LF_XSHUT_PIN, INPUT);
delay(10);
if (!lidar_LF.init())
{
Serial.println("Failed to detect and initialize lidar_LF!");
while (1) {}
}
//from Pololu forum post
lidar_LF.setMeasurementTimingBudget(20000);
lidar_LF.startContinuous();
lidar_LF.setAddress(DEFAULT_VL53L0X_ADDR + 4);
Serial.printf("lidar_LF address is 0x%x\n", lidar_LF.getAddress());
//now bring lidar_LC only out of reset, initialize it, and set its address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(LC_XSHUT_PIN, INPUT);
delay(10);
if (!lidar_LC.init())
{
Serial.println("Failed to detect and initialize lidar_LC!");
while (1) {}
}
//from Pololu forum post
lidar_LC.setMeasurementTimingBudget(20000);
lidar_LC.startContinuous();
lidar_LC.setAddress(DEFAULT_VL53L0X_ADDR + 5);
Serial.printf("lidar_LC address is 0x%x\n", lidar_LC.getAddress());
//now bring lidar_LR only out of reset, initialize it, and set its address
//input w/o pullups sets line to high impedance so XSHUT pullup to 3.3V takes over
pinMode(LR_XSHUT_PIN, INPUT);
if (!lidar_LR.init())
{
Serial.println("Failed to detect and initialize lidar_LR!");
while (1) {}
}
//from Pololu forum post
lidar_LR.setMeasurementTimingBudget(20000);
lidar_LR.startContinuous();
lidar_LR.setAddress(DEFAULT_VL53L0X_ADDR + 6);
Serial.printf("lidar_LR address is 0x%x\n", lidar_LR.getAddress());
Serial.printf("Msec\tLFront\tLCtr\tLRear\tRFront\tRCtr\tRRear\tLSteer\tRSteer\n");
}
void loop()
{
//from Pololu forum post
RF_Dist_mm = lidar_RF.readRangeContinuousMillimeters();
RC_Dist_mm = lidar_RC.readRangeContinuousMillimeters();
RR_Dist_mm = lidar_RR.readRangeContinuousMillimeters();
RightSteeringVal = (RF_Dist_mm - RR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
//from Pololu forum post
LF_Dist_mm = lidar_LF.readRangeContinuousMillimeters();
LC_Dist_mm = lidar_LC.readRangeContinuousMillimeters();
LR_Dist_mm = lidar_LR.readRangeContinuousMillimeters();
LeftSteeringVal = (LF_Dist_mm - LR_Dist_mm) / 100.f; //rev 06/21/20 see PPalace post
Serial.printf("%lu\t%d\t%d\t%d\t%d\t%d\t%d\t%2.2f\t%2.2f\n",
millis(),
LF_Dist_mm, LC_Dist_mm, LR_Dist_mm,
RF_Dist_mm, RC_Dist_mm, RR_Dist_mm,
LeftSteeringVal, RightSteeringVal);
}
and here's the results from the run
Opening port
Port open
lidar_RF address is 0x2a
lidar_RC address is 0x2b
lidar_RR address is 0x2c
lidar_LF address is 0x2d
lidar_LC address is 0x2e
lidar_LR address is 0x2f
Msec LFront LCtr LRear RFront RCtr RRear LSteer RSteer
1238 530 488 424 195 271 297 1.06 -1.02
1256 532 516 411 190 261 294 1.21 -1.04
1274 558 525 429 193 249 296 1.29 -1.03
1293 545 526 418 194 254 293 1.27 -0.99
1311 532 516 411 196 270 284 1.21 -0.88
1329 528 516 411 193 274 295 1.17 -1.02
1347 547 515 412 190 259 303 1.35 -1.13
1365 542 503 404 191 261 295 1.38 -1.04
1383 556 536 425 199 250 295 1.31 -0.96
1401 551 536 415 195 247 296 1.36 -1.01
1420 525 516 411 190 265 300 1.14 -1.10
1438 540 530 415 205 257 299 1.25 -0.94
1456 532 508 420 201 280 298 1.12 -0.97
1474 525 521 411 202 246 295 1.14 -0.93
1492 544 529 429 186 247 294 1.15 -1.08
1511 533 528 423 197 260 288 1.10 -0.91
1529 544 522 417 193 251 287 1.27 -0.94
1547 549 530 420 200 271 296 1.29 -0.96
1565 545 496 424 203 261 287 1.21 -0.84
1583 544 504 410 198 269 301 1.34 -1.03
1601 553 537 409 194 262 299 1.44 -1.05
1619 551 522 409 191 264 290 1.42 -0.99
1638 548 537 430 195 256 292 1.18 -0.97
1656 538 505 420 186 252 293 1.18 -1.07
1674 538 516 423 194 265 284 1.15 -0.90
1692 535 529 429 193 258 296 1.06 -1.03
1710 544 514 428 197 260 288 1.16 -0.91
1728 539 528 415 201 270 277 1.24 -0.76
1746 541 519 420 198 248 294 1.21 -0.96
1765 535 522 426 190 265 294 1.09 -1.04
1783 536 521 411 193 254 293 1.25 -1.00
1801 540 522 423 194 265 308 1.17 -1.14
1819 553 540 422 196 263 297 1.31 -1.01
Apparently, I can get results from all six sensors in the same time as for just one sensor. Anyone care to comment - is this too good to be true?
Frank
