Two-Way Traffic Control

#include <Arduino.h>
#include <Wire.h>
#include <NewPing.h>
#include <LiquidCrystal_I2C.h>


// Hoisted type definitions
enum TrafficState { IDLE, PENDING_A, PENDING_B, CONFIRM_A, CONFIRM_B, GREEN_A, GREEN_B };


// Forward declarations
bool vehicleDetected(NewPing &sonar);
bool sideAIsGreen();
bool sideBIsGreen();
void setLights();
long secondsForSide(bool isA, unsigned long now);
void writeLine(LiquidCrystal_I2C &lcd, byte row, const char *text);
void drawDisplays(unsigned long now, bool force);
void enterState(TrafficState next, unsigned long now);

const byte TRIG_A_PIN = 2;
const byte ECHO_A_PIN = 3;
const byte TRIG_B_PIN = 4;
const byte ECHO_B_PIN = 5;
const byte RELAY_A_PIN = 8;
const byte RELAY_B_PIN = 9;

const unsigned int DETECTION_CM = 30;
const unsigned int MAX_DISTANCE_CM = 200;
const unsigned long PENDING_MS = 5000UL;
const unsigned long GREEN_MS = 50000UL;
const unsigned long CONFIRM_MS = 3000UL;
const unsigned long SENSOR_INTERVAL_MS = 100UL;

NewPing sonarA(TRIG_A_PIN, ECHO_A_PIN, MAX_DISTANCE_CM);
NewPing sonarB(TRIG_B_PIN, ECHO_B_PIN, MAX_DISTANCE_CM);
LiquidCrystal_I2C lcdA(0x20, 16, 2);
LiquidCrystal_I2C lcdB(0x21, 16, 2);


TrafficState state = IDLE;
unsigned long stateStartedAt = 0;
unsigned long lastSensorAt = 0;
bool vehicleA = false;
bool vehicleB = false;
TrafficState lastDrawnState = IDLE;
long lastDrawnSeconds = -1;

bool vehicleDetected(NewPing &sonar) {
  unsigned int distance = sonar.ping_cm();
  return distance > 0 && distance < DETECTION_CM;
}

bool sideAIsGreen() {
  return state == GREEN_A;
}

bool sideBIsGreen() {
  return state == GREEN_B;
}

void setLights() {
  // HIGH energizes a relay. The SPDT relay then moves from red (NC) to green (NO).
  // The two conditions are mutually exclusive by construction.
  digitalWrite(RELAY_A_PIN, sideAIsGreen() ? HIGH : LOW);
  digitalWrite(RELAY_B_PIN, sideBIsGreen() ? HIGH : LOW);
}

const char *stateWordA() {
  if (state == PENDING_A || state == CONFIRM_A) return "PENDING";
  if (state == GREEN_A) return "GO";
  return "RED";
}

const char *stateWordB() {
  if (state == PENDING_B || state == CONFIRM_B) return "PENDING";
  if (state == GREEN_B) return "GO";
  return "RED";
}

long secondsForSide(bool isA, unsigned long now) {
  bool timed = (isA && (state == PENDING_A || state == CONFIRM_A || state == GREEN_A)) ||
               (!isA && (state == PENDING_B || state == CONFIRM_B || state == GREEN_B));
  if (!timed) return -1;

  unsigned long duration = (state == GREEN_A || state == GREEN_B) ? GREEN_MS :
                           (state == CONFIRM_A || state == CONFIRM_B) ? CONFIRM_MS : PENDING_MS;
  unsigned long elapsed = now - stateStartedAt;
  if (elapsed >= duration) return 0;
  return (long)((duration - elapsed + 999UL) / 1000UL);
}

void writeLine(LiquidCrystal_I2C &lcd, byte row, const char *text) {
  char padded[17];
  snprintf(padded, sizeof(padded), "%-16s", text);
  lcd.setCursor(0, row);
  lcd.print(padded);
}

void drawDisplays(unsigned long now, bool force) {
  long seconds = (state == PENDING_A || state == CONFIRM_A || state == GREEN_A) ? secondsForSide(true, now) :
                 (state == PENDING_B || state == CONFIRM_B || state == GREEN_B) ? secondsForSide(false, now) : -1;
  if (!force && state == lastDrawnState && seconds == lastDrawnSeconds) return;

  char line[17];
  snprintf(line, sizeof(line), "Side A %-9s", stateWordA());
  writeLine(lcdA, 0, line);
  if (secondsForSide(true, now) < 0) snprintf(line, sizeof(line), "Time -- B:%-7s", stateWordB());
  else snprintf(line, sizeof(line), "Time %2lds B:%-5s", secondsForSide(true, now), stateWordB());
  writeLine(lcdA, 1, line);

  snprintf(line, sizeof(line), "Side B %-9s", stateWordB());
  writeLine(lcdB, 0, line);
  if (secondsForSide(false, now) < 0) snprintf(line, sizeof(line), "Time -- A:%-7s", stateWordA());
  else snprintf(line, sizeof(line), "Time %2lds A:%-5s", secondsForSide(false, now), stateWordA());
  writeLine(lcdB, 1, line);

  lastDrawnState = state;
  lastDrawnSeconds = seconds;
}

void enterState(TrafficState next, unsigned long now) {
  state = next;
  stateStartedAt = now;
  setLights();
  drawDisplays(now, true);
}

void setup() {
  pinMode(RELAY_A_PIN, OUTPUT);
  pinMode(RELAY_B_PIN, OUTPUT);
  digitalWrite(RELAY_A_PIN, LOW);
  digitalWrite(RELAY_B_PIN, LOW);

  Wire.begin();
  lcdA.init();
  lcdA.backlight();
  lcdB.init();
  lcdB.backlight();

  unsigned long now = millis();
  enterState(IDLE, now);
}

void loop() {
  unsigned long now = millis();

  if (now - lastSensorAt >= SENSOR_INTERVAL_MS) {
    lastSensorAt = now;
    vehicleA = vehicleDetected(sonarA);
    vehicleB = vehicleDetected(sonarB);
  }

  switch (state) {
    case IDLE:
      if (vehicleA) enterState(PENDING_A, now);
      else if (vehicleB) enterState(PENDING_B, now);
      break;

    case PENDING_A:
      if (now - stateStartedAt >= PENDING_MS) enterState(CONFIRM_A, now);
      break;
    case PENDING_B:
      if (now - stateStartedAt >= PENDING_MS) enterState(CONFIRM_B, now);
      break;

    // A fresh reading inside the next three seconds confirms that a vehicle is still waiting.
    case CONFIRM_A:
      if (vehicleA) enterState(GREEN_A, now);
      else if (now - stateStartedAt >= CONFIRM_MS) enterState(IDLE, now);
      break;
    case CONFIRM_B:
      if (vehicleB) enterState(GREEN_B, now);
      else if (now - stateStartedAt >= CONFIRM_MS) enterState(IDLE, now);
      break;

    case GREEN_A:
      if (now - stateStartedAt >= GREEN_MS) enterState(PENDING_B, now);
      break;
    case GREEN_B:
      if (now - stateStartedAt >= GREEN_MS) enterState(PENDING_A, now);
      break;
  }

  drawDisplays(now, false);
}