Dual-Gate Speed Monitor

#include <Arduino.h>
#include <M5Cardputer.h>


// Hoisted type definitions
enum GateState : uint8_t { ARMED, WAITING_FOR_B };

void IRAM_ATTR onSensorA();
void IRAM_ATTR onSensorB();
void drawStaticScreen();
void drawStatus();
void drawMeasurement();

constexpr int SENSOR_A_PIN = 1;  // Grove G1 / white wire
constexpr int SENSOR_B_PIN = 2;  // Grove G2 / yellow wire
constexpr float BASELINE_CM = 10.0f;
constexpr float SPEED_LIMIT_CM_S = 35.0f;
constexpr uint32_t DEBOUNCE_US = 15000;
constexpr uint32_t MIN_TRAVEL_US = 3000;       // Reject impossible noise pulses.
constexpr uint32_t MAX_TRAVEL_US = 5000000;    // Reset an unfinished pass after 5 seconds.
constexpr uint16_t ALARM_TONE_HZ = 1800;
constexpr uint16_t ALARM_DURATION_MS = 300;



volatile GateState gateState = ARMED;
volatile uint32_t sensorATimeUs = 0;
volatile uint32_t lastAEdgeUs = 0;
volatile uint32_t lastBEdgeUs = 0;
volatile uint32_t travelTimeUs = 0;
volatile bool speedReady = false;

float lastSpeedCmS = 0.0f;
float lastTimeMs = 0.0f;
uint32_t speedingEvents = 0;
bool hasMeasurement = false;
bool overSpeed = false;
bool statusDirty = true;
bool measurementDirty = true;

void IRAM_ATTR onSensorA() {
  const uint32_t now = micros();
  if (now - lastAEdgeUs < DEBOUNCE_US) return;
  lastAEdgeUs = now;

  // Ignore another hit on A until B completes the current pass.
  if (gateState == ARMED) {
    sensorATimeUs = now;
    gateState = WAITING_FOR_B;
  }
}

void IRAM_ATTR onSensorB() {
  const uint32_t now = micros();
  if (now - lastBEdgeUs < DEBOUNCE_US) return;
  lastBEdgeUs = now;

  if (gateState == WAITING_FOR_B) {
    const uint32_t elapsed = now - sensorATimeUs;
    gateState = ARMED;
    if (elapsed >= MIN_TRAVEL_US && elapsed <= MAX_TRAVEL_US) {
      travelTimeUs = elapsed;
      speedReady = true;
    }
  }
}

void drawStaticScreen() {
  auto &display = M5Cardputer.Display;
  display.fillScreen(TFT_BLACK);
  display.setTextSize(2);
  display.setTextColor(TFT_CYAN, TFT_BLACK);
  display.setCursor(7, 5);
  display.print("STREET SPEED");
  display.drawFastHLine(0, 27, display.width(), TFT_DARKGREY);

  display.setTextSize(1);
  display.setTextColor(TFT_LIGHTGREY, TFT_BLACK);
  display.setCursor(7, 112);
  display.print("10 cm gate | Limit 35.0 cm/s");
  display.setCursor(7, 125);
  display.print("SPACE: clear event count");
}

void drawStatus() {
  auto &display = M5Cardputer.Display;
  display.fillRect(7, 34, 230, 17, TFT_BLACK);
  display.setTextSize(1);
  display.setCursor(7, 37);

  GateState state;
  noInterrupts();
  state = gateState;
  interrupts();

  if (state == WAITING_FOR_B) {
    display.setTextColor(TFT_YELLOW, TFT_BLACK);
    display.print("MEASURING: waiting for gate B");
  } else if (!hasMeasurement) {
    display.setTextColor(TFT_GREEN, TFT_BLACK);
    display.print("READY: pass vehicle through A then B");
  } else {
    display.setTextColor(overSpeed ? TFT_RED : TFT_GREEN, TFT_BLACK);
    display.print(overSpeed ? "SPEEDING EVENT RECORDED" : "LAST PASS: within limit");
  }
}

void drawMeasurement() {
  auto &display = M5Cardputer.Display;
  display.fillRect(7, 53, 232, 56, TFT_BLACK);
  display.setTextSize(1);
  display.setTextColor(TFT_LIGHTGREY, TFT_BLACK);
  display.setCursor(7, 55);
  display.print("LAST SPEED");

  display.setTextSize(3);
  display.setTextColor(hasMeasurement ? (overSpeed ? TFT_RED : TFT_GREEN) : TFT_DARKGREY, TFT_BLACK);
  display.setCursor(7, 67);
  if (hasMeasurement) {
    display.printf("%.1f", lastSpeedCmS);
  } else {
    display.print("--.-");
  }

  display.setTextSize(1);
  display.print(" cm/s");
  if (hasMeasurement) {
    display.setCursor(145, 79);
    display.setTextColor(TFT_LIGHTGREY, TFT_BLACK);
    display.printf("%.1f ms", lastTimeMs);
  }

  display.fillRect(155, 55, 83, 17, TFT_BLACK);
  display.setCursor(155, 57);
  display.setTextColor(TFT_YELLOW, TFT_BLACK);
  display.printf("Events: %lu", static_cast<unsigned long>(speedingEvents));
}

void setup() {
  auto cfg = M5.config();
  M5Cardputer.begin(cfg, true);
  M5Cardputer.Display.setRotation(1);

  pinMode(SENSOR_A_PIN, INPUT_PULLUP);
  pinMode(SENSOR_B_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(SENSOR_A_PIN), onSensorA, FALLING);
  attachInterrupt(digitalPinToInterrupt(SENSOR_B_PIN), onSensorB, FALLING);

  drawStaticScreen();
  drawStatus();
  drawMeasurement();
  statusDirty = false;
  measurementDirty = false;
}

void loop() {
  M5Cardputer.update();

  bool measured = false;
  uint32_t elapsedUs = 0;
  bool timedOut = false;
  noInterrupts();
  if (speedReady) {
    elapsedUs = travelTimeUs;
    speedReady = false;
    measured = true;
  }
  if (gateState == WAITING_FOR_B && micros() - sensorATimeUs > MAX_TRAVEL_US) {
    gateState = ARMED;
    timedOut = true;
  }
  interrupts();

  if (measured) {
    lastTimeMs = static_cast<float>(elapsedUs) / 1000.0f;
    lastSpeedCmS = BASELINE_CM * 1000000.0f / static_cast<float>(elapsedUs);
    overSpeed = lastSpeedCmS > SPEED_LIMIT_CM_S;
    hasMeasurement = true;
    if (overSpeed) {
      speedingEvents++;
      M5Cardputer.Speaker.tone(ALARM_TONE_HZ, ALARM_DURATION_MS);
    }
    statusDirty = true;
    measurementDirty = true;
  }

  if (timedOut) {
    statusDirty = true;
  }

  if (M5Cardputer.Keyboard.isKeyPressed(' ')) {
    speedingEvents = 0;
    statusDirty = true;
    measurementDirty = true;
    delay(180);  // One key press clears the count once.
  }

  if (statusDirty) {
    drawStatus();
    statusDirty = false;
  }
  if (measurementDirty) {
    drawMeasurement();
    measurementDirty = false;
  }

  delay(3);
}