Using 2 ToF4M Units as a Timing Gate

[chuck808]

Hello,

I am trying to use a pair of ToF4M Units through a PaHub, connected to the M5Stack Core2 with Arduino IDE, to create a timing gate but not quit getting the results I would like.

The included code snippet works fine/ish, however, the screen doesn't always display the speed when I pass my hand through the sensors (showing in the console readout), nor can I pass my hands through at speed. What am I missing or am I trying to push the units too far?

Thanks for any help.

#include <M5Core2.h>
#include <Wire.h>
#include "SparkFun_VL53L1X.h"
#include "Arduino.h"

#define TCAADDR 0x70 // Address for TCA9548A

// Declare sensors
SFEVL53L1X sensors[2]; // Array for sensors

// Line break configuration
int distanceThreshold = 1000; // Distance threshold for line break detection (adjusted for bicycles)
int timingBudget = 50; // Timing budget for better accuracy (reduced for high speed)

// Timing configuration
float sensorDistance = 30.0; // Distance between sensors in mm (adjust based on calibration)
unsigned long startMicros = 0; // Timer start time in microseconds
unsigned long stopMicros = 0; // Timer stop time in microseconds
unsigned long lastResetTime = 0; // Last time the screen was reset
unsigned long resetDelay = 1000000; // Delay for resetting screen (1 second in microseconds)
unsigned long minTimeDifference = 100; // Minimum time difference to avoid division by zero

// Global variables for trigger sequence tracking
bool sensor0Triggered = false;
bool sensor1Triggered = false;
unsigned long sensor0TriggerTime = 0;
unsigned long sensor1TriggerTime = 0;

// Debouncing configuration
const int debounceDelay = 50; // Debounce delay in milliseconds
unsigned long lastTriggerTime[2] = {0, 0}; // Last trigger time for each sensor

void tcaselect(uint8_t i) {
if (i > 5) return;
Wire.beginTransmission(TCAADDR);
Wire.write(1 << i);
Wire.endTransmission();
}

void setup() {
M5.begin();
Serial.begin(115200);
Wire.begin();
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.setTextSize(2);
Serial.println("Initializing Sensors...");

// Initialize sensors with timing budget
for (int i = 0; i < 2; i++) {
tcaselect(i);
if (sensors[i].begin() != 0) {
Serial.print("Sensor failed to initialize on port ");
Serial.println(i + 1);
} else {
Serial.print("Sensor initialized on port ");
Serial.println(i + 1);
sensors[i].setTimingBudgetInMs(timingBudget);
}
}

// Set up the display
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.setTextSize(5);
M5.Lcd.setTextColor(TFT_WHITE);
M5.Lcd.setTextDatum(MC_DATUM); // Center the text
}

void loop() {
M5.update();

unsigned long currentMicros = micros(); // Current time in microseconds

// Monitor sensor[0]
tcaselect(0);
sensors[0].startRanging();
while (!sensors[0].checkForDataReady()) {
delay(10);
}
int distance0 = sensors[0].getDistance();
sensors[0].clearInterrupt();
sensors[0].stopRanging();

// Check if sensor[0] is triggered with debouncing
if (distance0 > 0 && distance0 < distanceThreshold && currentMicros - lastTriggerTime[0] > debounceDelay * 1000) {
sensor0Triggered = true;
sensor0TriggerTime = currentMicros; // Record trigger time
Serial.println("Sensor 0 triggered"); // Debug output
lastTriggerTime[0] = currentMicros; // Update last trigger time
}

// Monitor sensor[1]
tcaselect(1);
sensors[1].startRanging();
while (!sensors[1].checkForDataReady()) {
delay(10);
}
int distance1 = sensors[1].getDistance();
sensors[1].clearInterrupt();
sensors[1].stopRanging();

// Check if sensor[1] is triggered with debouncing
if (distance1 > 0 && distance1 < distanceThreshold && currentMicros - lastTriggerTime[1] > debounceDelay * 1000) {
sensor1Triggered = true;
sensor1TriggerTime = currentMicros; // Record trigger time
Serial.println("Sensor 1 triggered"); // Debug output
lastTriggerTime[1] = currentMicros; // Update last trigger time
}

// Validate trigger sequence and calculate speed
if (sensor0Triggered && sensor1Triggered && sensor1TriggerTime > sensor0TriggerTime) {
unsigned long timeDifference = sensor1TriggerTime - sensor0TriggerTime;
if (timeDifference > 0) {
float speedInMmPerMs = sensorDistance / timeDifference * 1000.0;
float speedInKph = speedInMmPerMs * 3.6; // Convert to km/h
M5.Lcd.fillScreen(TFT_BLACK);
M5.Lcd.drawString(String(speedInKph, 2) + " km/h", 160, 120); // Display speed with 2 decimal places
lastResetTime = currentMicros; // Record reset time
}
// Reset trigger sequence
sensor0Triggered = false;
sensor1Triggered = false;
}

if (currentMicros - lastResetTime > resetDelay) {
M5.Lcd.fillScreen(TFT_BLACK);
startMicros = 0;
stopMicros = 0;
}

delay(100);
}