Lidar Add-On

Learn how to assemble the Lidar add-on for Stackr, read distance measurements from the VL53L0X time-of-flight (ToF) sensor, and build a hand follower that drives toward or away from your hand using the Stackr motors.

How to assemble the Lidar

The easiest way to assemble the Lidar is by following the instructions in the video below.

Reading distance with the ToF sensor

This sketch uses the Adafruit VL53L0X library to read distance in millimeters every 100 ms and print values to the Serial Monitor. Open the Serial Monitor at 115200 baud to view readings.

Stackr_Lidar.ino

#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

unsigned long lastMeasureTime = 0;
const unsigned long measureInterval = 100;

int distance = 100;

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);

  if (!lox.begin()) {
    Serial.println("Failed to find distance sensor!");
    while (1) delay(10);
  }
  Serial.println("Distance sensor initialized.");
}

void loop() {
  unsigned long currentTime = millis();
  if (currentTime - lastMeasureTime >= measureInterval) {
    lastMeasureTime = currentTime;

    VL53L0X_RangingMeasurementData_t measure;
    lox.rangingTest(&measure, false);

    if (measure.RangeStatus != 4) {
      Serial.print("Distance (mm): ");
      distance = measure.RangeMilliMeter;
      Serial.println(distance);
    } else {
      Serial.println("Out of range");
    }
  }
}

Code Breakdown

Library and Sensor: Wire.h sets up I2C on pins 21 (SDA) and 22 (SCL). The Adafruit VL53L0X object lox represents the distance sensor.

#include <Wire.h>
#include "Adafruit_VL53L0X.h"

Adafruit_VL53L0X lox = Adafruit_VL53L0X();

Sensor Init: lox.begin() starts the sensor. If it fails, the sketch prints an error and stops.

Wire.begin(21, 22);

if (!lox.begin()) {
  Serial.println("Failed to find distance sensor!");
  while (1) delay(10);
}

Timed Readings: A new measurement runs every 100 ms using millis(), not a blocking delay() in the main loop.

Range Status: RangeStatus == 4 means out of range; otherwise RangeMilliMeter holds the distance printed to Serial.

lox.rangingTest(&measure, false);

if (measure.RangeStatus != 4) {
  distance = measure.RangeMilliMeter;
  Serial.println(distance);
} else {
  Serial.println("Out of range");
}

Hand follower

This sketch combines the VL53L0X sensor with the SparkFun TB6612 motor library (same wiring as the Stackr motors tutorial). Stackr drives forward when your hand is farther than 100 mm, backs up when closer than 60 mm, and brakes in between. If nothing is in range, it drives forward to search.

Stackr_HandFollower.ino

#include <Wire.h>
#include <SparkFun_TB6612.h>
#include "Adafruit_VL53L0X.h"

#define AIN1 D5
#define BIN1 D6
#define AIN2 D4
#define BIN2 D7
#define PWMA D3
#define PWMB D8
#define STBY D9

const int fwdDirectionA = 1;
const int fwdDirectionB = 1;

Motor motor1 = Motor(AIN1, AIN2, PWMA, fwdDirectionA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, fwdDirectionB, STBY);

Adafruit_VL53L0X distanceSensor = Adafruit_VL53L0X();

#define SDA 21
#define SCL 22

unsigned long lastMeasureTime = 0;
const unsigned long measureInterval = 100;

void setup() {
  Serial.begin(115200);
  Wire.begin(SDA, SCL);

  if (!distanceSensor.begin()) {
    Serial.println("Failed to find distance sensor!");
    while (1) delay(10);
  }
  Serial.println("Distance sensor initialized.");
}

void loop() {
  unsigned long currentTime = millis();
  if (currentTime - lastMeasureTime >= measureInterval) {
    lastMeasureTime = currentTime;

    VL53L0X_RangingMeasurementData_t measure;
    distanceSensor.rangingTest(&measure, false);

    if (measure.RangeStatus != 4) {
      Serial.print("Distance (mm): ");
      int distance = measure.RangeMilliMeter;
      Serial.println(distance);

      if (distance > 100) {
        motor1.drive(150);
        motor2.drive(150);
      } else if (distance < 60) {
        motor1.drive(-150);
        motor2.drive(-150);
      } else {
        motor1.brake();
        motor2.brake();
      }
    } else {
      Serial.println("Out of range");
      motor1.drive(150);
      motor2.drive(150);
    }
  }
}

Hand Follower Code Breakdown

Motor Setup: The same pin definitions and Motor objects from the base Stackr sketch control both drive wheels.

Motor motor1 = Motor(AIN1, AIN2, PWMA, fwdDirectionA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, fwdDirectionB, STBY);

Distance Readings: Every 100 ms the sketch reads the ToF sensor, same as the distance example above.

Follow Logic: Farther than 100 mm → drive forward at speed 150. Closer than 60 mm → drive backward. Between 60 and 100 mm → brake and hold.

if (distance > 100) {
    motor1.drive(150);
    motor2.drive(150);
  } else if (distance < 60) {
    motor1.drive(-150);
    motor2.drive(-150);
  } else {
    motor1.brake();
    motor2.brake();
  }

Out of Range: When the sensor reports out of range, Stackr drives forward to look for a target again.

RobotiX One