4. Using 2 ToF4M Units as a Timing Gate

Using 2 ToF4M Units as a Timing Gate

  • C

    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);
    }

    0