Introduction

This year I participated in the RoboCup competition as a member of the Maze Leopard team (Rescue Maze category), and I was in charge of building the robot itself, which includes:

• Electronic design
• Sensor choice (distance, color, accel/gyro combo) + driver development
• Motor + motor driver choice
• Mechanical design
• Rescue kit dispenser design
• Reliability testing

I ended up choosing a pretty new sensor (for the time, which was a year ago), the VL53L5CX, which is a multizone distance sensor.

We would need 4 of these sensors running simultaneously on the robot to navigate the maze, so I needed to figure out how to use all of the sensors on the same bus in a reliable way.

I will probably go over why this sensor was chosen in another post, but I digress.

First tests

Before buying the sensor itself, I tried compiling and flashing a library I found online, just to see if it might work on the Teensy, but the example code immediately crashed because the library tried to access an instance of an object that didn’t yet exist (if I remember correctly).

It was at that moment that I decided that it was about time I started to write my own drivers, or at least understand how they work and how to improve upon them.

So I started digging in the code, and found out that all of the heavy lifting gets already done by ST, with their ULD (Ultra Lite Driver) API, and that to use it just a small number of functions need to be implemented in the platform.cpp file:

uint8_t RdByte(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAddress,
		uint8_t *p_value);

uint8_t WrByte(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAddress,
		uint8_t value);

uint8_t WrMulti(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAddress,
		uint8_t *p_values,
		uint32_t size);

uint8_t RdMulti(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAddress,
		uint8_t *p_values,
		uint32_t size);

uint8_t Reset_Sensor(
		VL53L5CX_Platform *p_platform);

void SwapBuffer(
		uint8_t 		*buffer,
		uint16_t 	 	 size);

uint8_t WaitMs(
		VL53L5CX_Platform *p_platform,
		uint32_t TimeMs);

At this point I was also motivated by the fact that the library didn’t support every function that ST’s API provides, and that could turn useful further down the line. Another advantage is the possibility of easily updating the ULD API to a more recent version.

You can find the implemented code for the platform.cpp file here.

I then created a small helper class that configures the sensor with my desired configuration, by directly calling the ULD API, available here.

Making multiple sensors work

To implement this functionality, the LPn pin needs to be pulled LOW on every sensor apart from the one that needs to have its address to be reprogrammed.

VL53L5CX_manager::VL53L5CX_manager(TwoWire &interface, bool cold_start) : resolution(0)
{
	if (cold_start)
	{
		for (uint8_t i = 0; i < SENSORS_NUM; i++)
		{
			pinMode(VL53L5CX_LPn_pin[i], OUTPUT);
			digitalWriteFast(VL53L5CX_LPn_pin[i], LOW); // Sensor stops listening
		}
	}
	else
	{
		for (uint8_t i = 0; i < SENSORS_NUM; i++)
		{
			pinMode(VL53L5CX_LPn_pin[i], OUTPUT);
			digitalWriteFast(VL53L5CX_LPn_pin[i], HIGH); // Sensor starts listening
		}
	}
	delay(100);
	for (uint8_t i = 0; i < SENSORS_NUM; i++)
	{
		digitalWriteFast(VL53L5CX_LPn_pin[i], HIGH); // Sensor starts listening
		Serial.print("Initializing VL53L5CX sensor ");
		Serial.println(i);
		sensors[i] = new ELIA(interface, VL53L5CX_addr[i]);

		if (sensors[i]->GetStatus() == 0)
		{
			Serial.print("VL53L5CX sensor ");
			Serial.print(i);
			Serial.print(" has address: 0x");
			Serial.println(VL53L5CX_addr[i], HEX);
		}
		else
		{
			Serial.print("VL53L5CX sensor ");
			Serial.print(i);
			Serial.println(" is disconnected");
			is_disconnected[i] = true;
		}

		digitalWriteFast(VL53L5CX_LPn_pin[i], LOW); // Sensor stops listening
		delay(100);											  // This delay is ESSENTIAL: If removed, the StartRanging function on the second sensor returns 255!!!
	}
	for (uint8_t i = 0; i < SENSORS_NUM; i++)
	{
		digitalWriteFast(VL53L5CX_LPn_pin[i], HIGH); // Sensor starts listening
	}
}

const uint8_t VL53L5CX_LPn_pin[4] = {
	29,	// Forward
	5,	// Backward
	6,		// Left
	27		// Right
};

const uint8_t VL53L5CX_int_pin[4] = {
	30,	// Forward
	4,	// Backward
	26,		// Left
	28		// Right
};

const uint8_t VL53L5CX_addr[4]{
	0x50U,
	0x60U,
	0x70U,
	0x80U,
};

Full implementation here

Be aware that the presence of some delay() statements is essential to proper sensor operation, because when the LPn pin goes back to HIGH the sensor needs some time to become fully operational again.

You can of course play around with the number of milliseconds and see what works for your application.

A weird issue I encountered was that sometimes the data would get ‘mixed’ between the sensors, which was really weird, this was caused by the fact that I assigned i2c addresses too close to each other, without knowing i2c uses 7 bit addressing.

In conclusion

If your multi-sensor confuguration doesn’t work, double check your connections, probably one of your jumper wires is bad, or if one of the sensors stops updating data after a couple of seconds of ranging the reason probably is because the supply voltage got too low for an instant.

Get a better power supply or try adding a capacitor to the output. In my case using a cheap LM2596S regulator from Amazon, a capacitor was needed to drive 4 sensors simultaneously.

These sensors are pretty power hungry compared to something like the SHARP GP2Y0A21YK0F and the current tends to spike quite a bit when in ranging mode, even more so with multiple actively ranging at the same time.

SIDE NOTE: I should probably take this code and release a library one day.