Lab 3: Time of Flight Sensors

Single ToF Sensor

The first ToF sensor was connected to the QWIIC breakout board and tested with the SparkFun ReadDistance example.

Figure: Single ToF sensor connected and returning distance readings via example sketch.

Figure: I2C scanner output showing device at 0x29. It matches the VL53L1X default. Note: When the IMU is also connected, a second device appears at 0x69 (ICM-20948 with AD0 high).

Sensor Mode Selection

The VL53L1X has three distance modes. Short mode has a maximum range of a pretty short 1.3m but is less affected by ambient light and provides faster, more reliable readings at close range. Medium mode extends to 3m, and Long mode reaches 4m but is more susceptible to ambient light interference and has slower ranging times. I chose to work with Short mode for now since it's more important for the final robot. I may change my mind on the placement of the second sensor, so I don't want to waste time working with it until I'm certain. But anyway, our main task is object collision prevention so having a good, well calibrated, fast sensor, then that's the best way to work towards our goal.

ToF Sensor Characterization

I tested the sensor accuracy and repeatability by placing a flat surface at known distances and collecting a reading at each distance. The sensor was tested at 100mm intervals from 100mm to 1000mm. I didn't have a ruler but printer paper is 29.7cm, so I folded it into thirds, flattened them, and laid 4 out in a line. I used a box and moved it away in those increments.

Figure: ToF measured distance vs. actual distance. The sensor tracks closely up to about 1000mm, with increasing error near the edge of Short mode range but almost none honestly.

Dual ToF Sensors

To use two ToF sensors simultaneously, I followed the process I outlined in prelab. There we some issues for many days as I worked on this, as often it would fail to detect the second sensor. It was at this time I rewired it from A5 to A2, because the connection to A5 was lacking and then completely broken when I tried to fix it.

// XSHUT Mech
pinMode(XSHUT_PIN, OUTPUT);
digitalWrite(XSHUT_PIN, LOW);   // keeps sensor 2 off
delay(10);

tof1.begin();                   // sensor 1 start at 0x29
tof1.setDistanceModeShort();
tof1.setI2CAddress(0x32);      // move sensor 1 to 0x32

digitalWrite(XSHUT_PIN, HIGH);  // sensor 2 turns on at 0x29, we leave it there
delay(10);
tof2.begin();
tof2.setDistanceModeShort();

Figure: Both ToF sensors returning independent distance readings simultaneously after XSHUT address reassignment succeeds.

Loop Speed and Limiting Factor

In order to measure how fast the main loop runs, I'm having the code print the Artemis clock as fast as possible and only prints ToF data when checkForDataReady() returns true. The loop itself runs extremely fast without the data since it's none blocking. As expected, the limitation is the ToF sensor ranging time. In Short mode, each sensor produces a new reading roughly every ~50ms (about 20 readings per second per sensor) which seems fast until you consider that the IMU samples much faster. The key with this model is that it doesn't check if the info is there, it just runs regardless.

 
while (millis() - start < 10000 && tof_count < BUF_LEN) {
    bool got1 = false, got2 = false;
    if (tof1.checkForDataReady()) {
        tof1_buf[tof_count] = tof1.getDistance();
        tof1.clearInterrupt();
        got1 = true;
    }
    if (tof2.checkForDataReady()) {
        tof2_buf[tof_count] = tof2.getDistance();
        tof2.clearInterrupt();
        got2 = true;
    }
    if (got1 || got2) {
        tof_time_buf[tof_count] = millis();
        tof_count++;
    }
}

Figure: Speed test output showing loop rate vs. ToF reading rate. The loop notably runs far faster than the sensors produce data, confirming the sensors are the bottleneck.

ToF Data Over Bluetooth

Time-stamped ToF data from both sensors was recorded into on-board buffers for 10 seconds, then transmitted over BLE to the laptop. Then, the data was parsed in the Jupyter notification handler and plotted with matplotlib.

# Jupyter: receive and parse ToF data
tof_time = []; tof1_data = []; tof2_data = []

def tof_handler(uuid, notif):
    s = ble.bytearray_to_string(notif).strip()
    if s == "TOF_DONE":
        print("TOF done, received:", len(tof_time))
        return
    parts = s.split("|")
    t = int(parts[0].split(":")[1])
    a = int(parts[1].split(":")[1])
    b = int(parts[2].split(":")[1])
    tof_time.append(t); tof1_data.append(a); tof2_data.append(b)

ble.start_notify(ble.uuid["RX_STRING"], tof_handler)
ble.send_command(CMD.SEND_TOF, "")

Figure: Time vs Distance plot for both ToF sensors, from data collected over BLE. Sensors show independent readings of my hand over 10 seconds. I begin by putting it in view around 2 seconds in, then moving away slowly. At the end, one sensor slips out of my grasp and is out of alignment.

IMU Data Over Bluetooth

IMU data (accelerometer and gyroscope) was recorded into buffers for 10 seconds and sent over BLE using the same buffered approach. The notification handler parses the T:|AX:|AY:|AZ:|GX:|GY:|GZ: format into separate lists.

# Jupyter: receive and parse IMU data
imu_time = []
ax_data = []; ay_data = []; az_data = []
gx_data = []; gy_data = []; gz_data = []

def imu_handler(uuid, notif):
    s = ble.bytearray_to_string(notif).strip()
    if s == "IMU_DONE":
        print("IMU done, received:", len(imu_time))
        return
    parts = s.split("|")
    vals = {}
    for p in parts:
        k, v = p.split(":")
        vals[k] = float(v)
    imu_time.append(int(vals["T"]))
    ax_data.append(vals["AX"]); ay_data.append(vals["AY"])
    az_data.append(vals["AZ"]); gx_data.append(vals["GX"])
    gy_data.append(vals["GY"]); gz_data.append(vals["GZ"])

ble.start_notify(ble.uuid["RX_STRING"], imu_handler)
ble.send_command(CMD.SEND_IMU, "")

Figure: Time vs Angle/Acceleration plot for the IMU. Top: accelerometer (X, Y, Z in mg). Bottom: gyroscope (X, Y, Z in dps). Data sent over BLE from on-board buffers.