const int RED_PINS[] = {2, 5}; // 0: N+S | 1: E+W
const int YELLOW_PINS[] = {3, 6};
const int GREEN_PINS[] = {4, 7};
const int CAM_PINS[] = {8, 9, 10, 11}; // N, S, E, W camera flash LEDs
const char* LANE_NAMES[] = {"NORTH", "SOUTH", "EAST", "WEST"};
int laneCars[] = {0, 0, 0, 0};
int laneViol[] = {0, 0, 0, 0};
int activeAxis = 0;
bool isYellowPhase = false;
unsigned long phaseStart = 0;
unsigned long greenDuration = 5000;
unsigned long camFlashUntil[] = {0, 0, 0, 0};
const unsigned long MAX_GREEN_MS = 15000;
const unsigned long YELLOW_TIME_MS = 2000;
void setAxisLights(int axis, bool r, bool y, bool g) {
digitalWrite(RED_PINS[axis], r ? HIGH : LOW);
digitalWrite(YELLOW_PINS[axis], y ? HIGH : LOW);
digitalWrite(GREEN_PINS[axis], g ? HIGH : LOW);
}
void setup() {
Serial.begin(115200);
for (int axis = 0; axis < 2; axis++) {
pinMode(RED_PINS[axis], OUTPUT);
pinMode(YELLOW_PINS[axis], OUTPUT);
pinMode(GREEN_PINS[axis], OUTPUT);
setAxisLights(axis, true, false, false);
}
for (int i = 0; i < 4; i++) {
pinMode(CAM_PINS[i], OUTPUT);
digitalWrite(CAM_PINS[i], LOW);
}
phaseStart = millis();
}
void loop() {
unsigned long now = millis();
// 1. Read "N,S,E,W:vN,vS,vE,vW" from Webots
if (Serial.available()) {
String msg = Serial.readStringUntil('\n');
msg.trim();
if (msg.length() > 0) {
laneViol[0] = laneViol[1] = laneViol[2] = laneViol[3] = 0;
if (msg.indexOf(':') != -1) {
sscanf(msg.c_str(), "%d,%d,%d,%d:%d,%d,%d,%d",
&laneCars[0], &laneCars[1], &laneCars[2], &laneCars[3],
&laneViol[0], &laneViol[1], &laneViol[2], &laneViol[3]);
} else {
sscanf(msg.c_str(), "%d,%d,%d,%d",
&laneCars[0], &laneCars[1], &laneCars[2], &laneCars[3]);
}
}
}
int axisCars[2];
axisCars[0] = laneCars[0] + laneCars[1]; // North + South
axisCars[1] = laneCars[2] + laneCars[3]; // East + West
unsigned long elapsed = now - phaseStart;
if (!isYellowPhase) {
if (axisCars[activeAxis] > 0 && elapsed < MAX_GREEN_MS && greenDuration < elapsed + 2000) {
greenDuration = min(MAX_GREEN_MS, elapsed + 2000);
}
if (elapsed >= greenDuration) {
isYellowPhase = true;
phaseStart = now;
}
} else if (elapsed >= YELLOW_TIME_MS) {
isYellowPhase = false;
phaseStart = now;
int otherAxis = 1 - activeAxis;
if (axisCars[otherAxis] > 0 || axisCars[activeAxis] == 0) {
activeAxis = otherAxis;
}
greenDuration = min(MAX_GREEN_MS, 4000UL + axisCars[activeAxis] * 2000UL);
}
for (int axis = 0; axis < 2; axis++) {
setAxisLights(axis,
axis != activeAxis,
(axis == activeAxis && isYellowPhase),
(axis == activeAxis && !isYellowPhase));
}
for (int lane = 0; lane < 4; lane++) {
int laneAxis = (lane < 2) ? 0 : 1;
if ((laneAxis != activeAxis) && laneViol[lane] == 1 && now > camFlashUntil[lane]) {
camFlashUntil[lane] = now + 1500;
laneViol[lane] = 0;
}
digitalWrite(CAM_PINS[lane], (now < camFlashUntil[lane]) ? HIGH : LOW);
}
// 2. Send current signal state back to Webots
Serial.print("STATE:");
Serial.print(activeAxis);
Serial.print(",");
Serial.println(isYellowPhase ? 1 : 0);
delay(100);
}
East pole camera
South pole camera
North pole camera
West pole camera