1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

/* Global Enums */

// Represents whether code is in automatic or override state, auto by default
enum OperationalMode { AUTOMATIC, OVERRIDE };

// Represents whether the bridge is opened, closed or in an unknown state
enum BridgeGateState { CLOSED, OPEN, UNKNOWN };

// Represents state of boat and road lights
enum LightColours { RED, GREEN, YELLOW, NONE, ALL };

// Represents whether bridge lights should be on or off
enum BridgeLightState { ON, OFF };

// Represents whether the buzzer sound should be on or off
enum BuzzerSound { SOUND, NO_SOUND };

// Represents which motor to move
enum Motor { BRIDGE, GATE };

// Represents which sensor to read from
enum AdcValue { PHOTO_RESISTOR, BRIDGE_WEIGHT };

// Bridge sequence state
enum BridgeSequenceState {
  IDLE,
  OPENING_FOR_BOATS,
  BOATS_PASSING,
  CLOSING_FOR_CARS,
  CARS_PASSING,
  DIAGNOSTIC,
  TESTING
};

// Represents which stage the bridge is at while moving
enum BridgeMovementState { STATE_ONE, STATE_TWO, STATE_THREE, STATE_FOUR, STATE_FIVE, RECOVERY, SUCCESS };

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

// Tasks for Multithreading
TaskHandle_t AutomaticModeTask;
TaskHandle_t BridgeControlTask;
TaskHandle_t GateControlTask;
TaskHandle_t OverrideQueueTask;

// Create tasks
xTaskCreatePinnedToCore(automaticModeTask,   // Task function
                        "AutomaticMode",     // Name
                        10000,               // Stack size
                        NULL,                // Parameter
                        2,                   // Priority (higher than motor tasks)
                        &AutomaticModeTask,  // Task handle
                        1);                  // Core 1 (Application CPU)

xTaskCreatePinnedToCore(bridgeControlTask,   // Task function
                        "BridgeControl",     // Name
                        4096,                // Stack size
                        NULL,                // Parameter
                        1,                   // Priority
                        &BridgeControlTask,  // Task handle
                        0);                  // Core 0 (Protocol CPU)

xTaskCreatePinnedToCore(gateControlTask,   // Task function
                        "GateControl",     // Name
                        4096,              // Stack size
                        NULL,              // Parameter
                        1,                 // Priority
                        &GateControlTask,  // Task handle
                        0);                // Core 0 (Protocol CPU)

xTaskCreatePinnedToCore(overrideQueueTask,   // Task function
                        "OverrideQueue",     // Name
                        4096,                // Stack size
                        NULL,                // Parameter
                        3,                   // Priority (highest - processes overrides)
                        &OverrideQueueTask,  // Task handle
                        1);                  // Core 1 (Application CPU)

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17

// Command structures
struct OverrideCommand {
  char command[50];
};

struct BridgeCommand {
  BridgeGateState desiredState;
};

struct GateCommand {
  BridgeGateState desiredState;
};

// Create queues
QueueHandle_t bridgeCommandQueue = xQueueCreate(5, sizeof(BridgeCommand));
QueueHandle_t gateCommandQueue = xQueueCreate(5, sizeof(GateCommand));
QueueHandle_t pendingOverrideQueue = xQueueCreate(10, sizeof(OverrideCommand));

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20

void changeBridgeStateAsync(BridgeGateState desiredMode) {
  // Check if enough time has passed since last bridge state change
  extern unsigned long lastBridgeStateChange;
  unsigned long currentTime = millis();

  if (currentTime - lastBridgeStateChange < BRIDGE_STATE_CHANGE_COOLDOWN) {
    Serial.println("INFO: Bridge state change blocked, cooldown period active");
    sendUpdate("SYSTEM_UPDATE: dequeued_last_message");
    return;
  }

  extern BridgeGateState currentBridgeState;
  extern QueueHandle_t bridgeCommandQueue;

  // Update the queue if the desired and current states aren't the same
  if (desiredMode != currentBridgeState) {
    BridgeCommand cmd = {desiredMode};
    xQueueSend(bridgeCommandQueue, &cmd, 0);
  }
}

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146

void bridgeControlTask(void *parameter) {
  // Command can either be OPEN or CLOSED
  BridgeCommand cmd;

  // Runs forever
  for (;;) {
    // When a command is received (either from automatic mode or override) it wakes up
    if (xQueueReceive(bridgeCommandQueue, &cmd, portMAX_DELAY)) {
      // Take mutex before changing state
      if (xSemaphoreTake(stateMutex, portMAX_DELAY)) {
        // ... If desired state is CLOSED

        else if (cmd.desiredState == OPEN && currentBridgeState != OPEN) {
          // Check weight on bridge
          Serial.println("INFO: Checking weight on the bridge");

          // Release for weight check
          xSemaphoreGive(stateMutex);

          long weightReading = checkBridgeWeight();

          // Send weight reading to Java
          sendUpdate("WEIGHT_CHECK: " + String(weightReading));

          // Re-acquire lock
          xSemaphoreTake(stateMutex, portMAX_DELAY);

          // ... weight checking logic

          Serial.println("INFO: Weight check passed (" + String(weightReading) + "), opening bridge");

          // Rotate the motor to open the bridge
          rotateMotorForOpen(BRIDGE);

          // Release mutex briefly to allow motor to start
          xSemaphoreGive(stateMutex);
          vTaskDelay(200 / portTICK_PERIOD_MS);
          xSemaphoreTake(stateMutex, portMAX_DELAY);

          // Update the timestamp when bridge state change begins
          lastBridgeStateChange = millis();
          // Track how long it takes to open the bridge
          unsigned long operationStartTime = millis();
          // Release mutex
          xSemaphoreGive(stateMutex);

          // Read how far away the bridge is
          long bridgeDistance;

          // Booleans to keep track of errors
          bool bridgeIsOpen = false;
          bool timeoutOccurred = false;

          // Greater than bridge open distance since bridge is moving closer
          // Bridge starts at 15cm (CLOSED), moves to 1cm (OPEN)
          while (bridgeDistance > BRIDGE_OPEN_DISTANCE) {
            // Timeout protection
            if (millis() - operationStartTime > BRIDGE_TIMEOUT) {
              timeoutOccurred = true;
              break;
            }

            // Stop motor for stable reading (avoid EMI interference)
            stopMotor(BRIDGE);

            // Wait for motor to fully stop and EMI to settle
            vTaskDelay(150 / portTICK_PERIOD_MS);

            // Take multiple readings and average them to filter out noise/EMI
            long reading1, reading2, reading3;
            bool valid1, valid2, valid3;

            // Calculate average of valid readings
            int validCount = 0;
            long sum = 0;

            if (valid1) {
              sum += reading1;
              validCount++;
            }
            if (valid2) {
              sum += reading2;
              validCount++;
            }
            if (valid3) {
              sum += reading3;
              validCount++;
            }

            if (validCount == 0) {
              Serial.println("ERROR: All sensor readings invalid - EMI or sensor failure");
              break;
            }

            bridgeDistance = sum / validCount;

            if (validCount < 3) {
              Serial.print("WARNING: Only ");
              Serial.print(validCount);
              Serial.println("/3 readings valid, using average");
            }

            Serial.print("INFO: Bridge distance (averaged): ");
            Serial.print(bridgeDistance);
            Serial.println(" cm");

            // Only restart motor if bridge hasn't reached target
            if (bridgeDistance > BRIDGE_OPEN_DISTANCE) {
              rotateMotorForOpen(BRIDGE);
              // Give motor time to build momentum before next check
              vTaskDelay(200 / portTICK_PERIOD_MS);
            }
          }

          // Check if we successfully reached open position
          if (bridgeDistance <= BRIDGE_OPEN_DISTANCE && !timeoutOccurred) {
            bridgeIsOpen = true;
          }

          if (xSemaphoreTake(stateMutex, portMAX_DELAY)) {
            // Stop the motor and update the bridge state
            stopMotor(BRIDGE);

            if (bridgeIsOpen) {
              currentBridgeState = OPEN;
              Serial.println("INFO: Bridge is OPEN");
            } else if (timeoutOccurred) {
              currentBridgeState = UNKNOWN;
              Serial.println("ERROR: Bridge state UNKNOWN after timeout");
            } else {
              currentBridgeState = UNKNOWN;
              Serial.println("ERROR: Bridge state UNKNOWN due to sensor failure or unknown error");
            }

            // Release mutex
            xSemaphoreGive(stateMutex);
          }
        }
        // If it's another command then release the mutex
        else {
          xSemaphoreGive(stateMutex);
        }
      }
    }
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21

// Mutex declaration
SemaphoreHandle_t stateMutex;

// In automatic mode task (Core 1)
if (xSemaphoreTake(stateMutex, 100 / portTICK_PERIOD_MS)) {
  // Safe to read/write shared state
  if (currentOperationalMode == AUTOMATIC) {
    switch (currentSequenceState) {
      // ... The state machine logic
    }
  }
  xSemaphoreGive(stateMutex);
}

// In motor control task (Core 0)
if (xSemaphoreTake(stateMutex, portMAX_DELAY)) {
  stopMotor(BRIDGE);
  // Updating a shared variable
  currentBridgeState = OPEN;
  xSemaphoreGive(stateMutex);
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

void automaticModeTask(void *parameter) {
  TickType_t xLastWakeTime = xTaskGetTickCount();
  
  for (;;) {
    if (xSemaphoreTake(stateMutex, 100 / portTICK_PERIOD_MS)) {
      if (currentOperationalMode == AUTOMATIC && 
          !isBridgeMovementExecutingInOverride) {
        
        switch (currentSequenceState) {
          case IDLE:
            // Check for traffic and initiate sequences
            if (canChangeBridge) {
              if (currentBridgeState == CLOSED) {
                long boatDistance = readUltrasonicDistanceBoatTraffic();
                if (isValidSensorReading(boatDistance) && 
                    boatDistance < BOATS_WAITING_DISTANCE) {
                  currentSequenceState = OPENING_FOR_BOATS;
                  sequenceStartTime = millis();
                }
              }
              // Check for cars when bridge is open...
            }
            break;
            
          case OPENING_FOR_BOATS:
            // Sequence with timing and safety checks
            break;
            
          case BOATS_PASSING:
            // Wait 10 seconds before checking for more traffic
            break;
            
          case CLOSING_FOR_CARS:
            // Similar to OPENING_FOR_BOATS but in reverse
            break;
            
          case CARS_PASSING:
            // Wait 10 seconds before returning to IDLE
            break;
            
          case DIAGNOSTIC:
            // Attempt position recovery
            break;
        }
      }
      xSemaphoreGive(stateMutex);
    }
    vTaskDelayUntil(&xLastWakeTime, 500 / portTICK_PERIOD_MS);
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82

case OPENING_FOR_BOATS:
  // Execute the sequence for opening the bridge
  {
    // currentMovementState is used here to ensure the functions aren't called again causing
    // cooldown blocks
    unsigned long elapsed = millis() - sequenceStartTime;
    // After 2s turn road traffic lights yellow
    if (elapsed >= 2000 && currentMovementState == STATE_ONE) {
      // After 2 seconds set the road lights to yellow
      setRoadTrafficLights(YELLOW);
      currentMovementState = STATE_TWO;
    }
    // After 8s set road traffic lights to red and close the gate
    else if (elapsed >= 8000 && currentMovementState == STATE_TWO) {
      // After 8 seconds set the road lights to red and close the gates
      setRoadTrafficLights(RED);
      changeGateStateAsync(CLOSED);
      currentMovementState = STATE_THREE;
    }
    // After 14s open bridge, 6s for gate to close (5s transition + 1s buffer)
    else if (elapsed >= 14000 && currentMovementState == STATE_THREE) {
      changeBridgeStateAsync(OPEN);
      currentMovementState = STATE_FOUR;
    }
    // After 25s, Check results (14s + 11s for bridge to open and stabilize)
    else if (elapsed >= 25000 && currentMovementState == STATE_FOUR) {
      // Reset movement state
      currentMovementState = STATE_ONE;
      // Go into diagnostics mode if the bridge state is unknown
      if (currentBridgeState == UNKNOWN) {
        currentSequenceState = DIAGNOSTIC;
        break;
      }

      // Allow up to 13 more seconds for the bridge to fully stop moving
      if (currentBridgeState != OPEN && elapsed < 38000) {
        currentMovementState = STATE_FOUR;
        break;
      }

      // If for some reason the bridge didn't open then set boat lights to red, open gate and go into RECOVERY
      if (currentBridgeState != OPEN) {
        setBoatTrafficLights(RED);
        changeGateStateAsync(OPEN);
        currentMovementState = RECOVERY;

        Serial.println("Info: Bridge state mismatch, expected OPEN, returning to CARS_PASSING");
        sendUpdate("SYSTEM_UPDATE: bridge_state_mismatch");
        break;
      }

      // Bridge is open, set boat lights to green
      setBoatTrafficLights(GREEN);
      currentMovementState = SUCCESS;
    }
    // After 30s If the bridge is in recovery state (bridge remains closed), set road lights to green and
    // go into CARS_PASSING
    else if (elapsed >= 30000 && currentMovementState == RECOVERY) {
      // Ensure gate is open before transitioning to CARS_PASSING
      changeGateStateAsync(OPEN);

      // Release mutex during delay
      xSemaphoreGive(stateMutex);
      // Wait 5s for gate to open
      vTaskDelay(GATE_TRANSITION_INTERVAL / portTICK_PERIOD_MS);
      xSemaphoreTake(stateMutex, portMAX_DELAY);

      setRoadTrafficLights(GREEN);

      currentSequenceState = CARS_PASSING;
      currentMovementState = STATE_ONE;
      sequenceStartTime = millis();
    }
    // After 25s if bridge has opened, set boat traffic lights to green and go into BOATS_PASSING
    else if (elapsed >= 25000 && currentMovementState == SUCCESS) {
      currentSequenceState = BOATS_PASSING;
      currentMovementState = STATE_ONE;
      sequenceStartTime = millis();
      Serial.println("Bridge opened: Waiting for boats to pass");
    }
  }
  break;

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

// Function to update the lights via shift register
void updateLights() {
  // Set all lights off to avoid the issue with lights not turning off properly
  // Shift registers don't seem to dynamically update the lights so 0 them out first
  digitalWrite(LATCH_PIN, LOW);
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, 0);
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, 0);
  digitalWrite(LATCH_PIN, HIGH);

  // Start by setting the latch low
  digitalWrite(LATCH_PIN, LOW);

  // Shift out the byte to the shift register
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, lightStatesRegister2);
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, lightStatesRegister1);

  // Latch the data to the outputs
  digitalWrite(LATCH_PIN, HIGH);
}

// Set road/boat traffic lights
void setRoadTrafficLights(LightColours desiredColour) {
  // Clear bits 0, 1, 2 (road traffic light bits)
  lightStatesRegister1 = (lightStatesRegister1 & 0b00011111);
  Serial.print("INFO: Current road traffic light state: ");

  // Update tracking variable
  currentRoadTrafficLight = desiredColour;

  // Set the desired light using |= which is a bitwise OR operator
  switch (desiredColour) {
    case RED:
      // Bit 0
      lightStatesRegister1 |= 0b00100000;
      Serial.println("Red");
      break;
    case YELLOW:
      // Bit 1
      lightStatesRegister1 |= 0b01000000;
      Serial.println("Yellow");
      break;
    case GREEN:
      // Bit 2
      lightStatesRegister1 |= 0b10000000;
      Serial.println("Green");
      break;
    case NONE:
      // All off
      lightStatesRegister1 |= 0b00000000;
      Serial.println("None");
      break;
    case ALL:
      // All on (Bits 0-2)
      lightStatesRegister1 |= 0b11100000;
      Serial.println("All");
      break;
  }
  // Update shift register
  updateLights();
}

void setBoatTrafficLights(LightColours desiredColour) {
  // Clear bits 3 and 4 (boat light bits)
  lightStatesRegister1 = (lightStatesRegister1 & 0b11100111);
  Serial.print("INFO: Current boat traffic light state: ");

  // Update tracking variable
  currentBoatTrafficLight = desiredColour;

  // Set the desired light
  switch (desiredColour) {
    case RED:
      // Bit 3
      lightStatesRegister1 |= 0b00001000;
      Serial.println("Red");
      break;
    case GREEN:
      // Bit 4
      lightStatesRegister1 |= 0b00010000;
      Serial.println("Green");
      break;
    case NONE:
      // All off
      lightStatesRegister1 |= 0b00000000;
      Serial.println("None");
      break;
    case ALL:
      // All on (Bits 3-4)
      lightStatesRegister1 |= 0b00011000;
      Serial.println("All");
      break;
  }
  // Update shift register
  updateLights();
}

void setBridgeLights(BridgeLightState desiredState) {
  if (currentBridgeLightState != desiredState) {
    currentBridgeLightState = desiredState;
    switch (desiredState) {
      case ON:
        lightStatesRegister1 |= 0b00000111;
        lightStatesRegister2 = 0b11111111;
        updateLights();
        Serial.println("INFO: Current bridge light state: on");
        break;
      case OFF:
        lightStatesRegister1 &= 0b11111000;
        lightStatesRegister2 = 0b00000000;
        updateLights();
        Serial.println("INFO: Current bridge light state: off");
        break;
    }
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

long readUltrasonicDistanceBoatTraffic() {
  // Send a 10-microsecond pulse to the trigPin
  digitalWrite(BOAT_TRAFFIC_TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(BOAT_TRAFFIC_TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(BOAT_TRAFFIC_TRIG_PIN, LOW);

  // Measure the time it takes for the pulse to return on the echoPin - 30ms timeout
  long duration = pulseIn(BOAT_TRAFFIC_ECHO_PIN, HIGH, 30000);

  // If no valid reading, return a large distance (ie no boats)
  if (duration == 0) {
    Serial.println("INFO: Boat traffic no sensor connected or invalid reading");
    return 9999;
  }

  // Calculate the distance in centimeters
  // Speed of sound is 0.0343 cm/us
  long distance = (duration * 0.0343) / 2;

  Serial.print("INFO: Boat traffic distance in cm: ");
  Serial.println(distance);

  // Return the distance value
  return distance;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11

// Function to read adc values
long readAdcSensorValue(AdcValue sensor) {
  switch (sensor) {
    case PHOTO_RESISTOR:
      return analogRead(PHOTO_RESISTOR_PIN);
    case BRIDGE_WEIGHT:
      return analogRead(BRIDGE_WEIGHT_SENSOR_PIN);
    default:
      return 0;
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

// Rotation for opening --> Can either be BRIDGE or GATE
void rotateMotorForOpen(Motor motorToMove) {
  // Rotate the motor in one direction (600 microseconds, clockwise)
  int dutyCycleCW = (600 / 20000.0) * 255;

  // Rotate according motor, make noise through buzzer and print in console
  switch (motorToMove) {
    case BRIDGE:
      // Make noise
      setBuzzer(SOUND);
      // Set PWM signal for clockwise direction
      ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleCW);
      Serial.println("INFO: Opening the bridge");
      break;
    case GATE:
      // Set PWM signal for clockwise direction
      ledcWrite(PWM_CHANNEL_GATE, dutyCycleCW);
      Serial.println("INFO: Opening the gate");
      break;
  }
}

// Rotation for closing --> Can either be BRIDGE or GATE
void rotateMotorForClose(Motor motorToMove) {
  // Rotate the motor in the opposite direction (2000 microseconds, counter-clockwise)
  int dutyCycleACW = (2000 / 20000.0) * 255;

  // Rotate according motor, make noise through buzzer and print in console
  switch (motorToMove) {
    case BRIDGE:
      // Make noise
      setBuzzer(SOUND);
      // Set PWM signal for counter-clockwise direction
      ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleACW);
      Serial.println("INFO: Closing the bridge");
      break;
    case GATE:
      // Set PWM signal for counter-clockwise direction
      ledcWrite(PWM_CHANNEL_GATE, dutyCycleACW);
      Serial.println("INFO: Closing the gate");
      break;
  }
}

// Stop motor --> Can either be BRIDGE or GATE
void stopMotor(Motor motorToMove) {
  // Stop the motor (1500 microseconds, neutral)
  int dutyCycleStop = (1500 / 20000.0) * 255;

  switch (motorToMove) {
    case BRIDGE:
      setBuzzer(NO_SOUND);
      ledcWrite(BUZZER_CHANNEL, 0);
      // Set PWM signal for stop
      ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleStop);
      Serial.println("INFO: Stopping the bridge motor");
      break;
    case GATE:
      // Set PWM signal for stop
      ledcWrite(PWM_CHANNEL_GATE, dutyCycleStop);
      Serial.println("INFO: Stopping the gate motor");
      break;
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

#include "communication/communication.h"

#include "config/config.h"
#include "sensors/sensors.h"
#include "types/types.h"

// External global variables
extern WiFiUDP udp;
extern OperationalMode currentOperationalMode;
extern BridgeGateState currentBridgeState;
extern BridgeGateState currentGateState;
extern BridgeSequenceState currentSequenceState;
extern BridgeMovementState currentMovementState;
extern LightColours currentRoadTrafficLight;
extern LightColours currentBoatTrafficLight;
extern BridgeLightState currentBridgeLightState;
extern unsigned long totalBridgeOpenings;
extern unsigned long totalBridgeClosings;
extern unsigned long faultCount;
extern unsigned long systemStartTime;
extern bool isBridgeMovementExecutingInOverride;
extern bool manualBridgeLights;
extern QueueHandle_t pendingOverrideQueue;
extern SemaphoreHandle_t stateMutex;

/* Communication Function */

// Function to send updates via UDP
void sendUpdate(String msg) {
  Serial.println("INFO: Sending message to Java Program: " + msg);
  udp.beginPacket(remoteIP, remotePort);
  udp.print(msg);
  udp.endPacket();
}

// Status update function
void sendStatusUpdate() {
  // Acquire mutex (runs on main loop meaning the default Core 1 Application CPU)
  if (xSemaphoreTake(stateMutex, 50 / portTICK_PERIOD_MS)) {
    // Read current distances
    long roadDistance = readUltrasonicDistanceRoadTraffic();
    long boatDistance = readUltrasonicDistanceBoatTraffic();
    long bridgeMovementDistance = readUltrasonicDistanceBridgeMovement();
    long boatClearanceDistance = readUltrasonicDistanceBoatClearance();

    // Create the status message
    String statusMsg = "STATUS: ";
    statusMsg += "MODE:" + String(operationalModeToString(currentOperationalMode)) + "|";
    statusMsg += "BRIDGE:" + String(bridgeGateStateToString(currentBridgeState)) + "|";
    statusMsg += "GATE:" + String(bridgeGateStateToString(currentGateState)) + "|";
    statusMsg += "ROAD_DISTANCE:" + String(roadDistance) + "|";
    statusMsg += "BOAT_DISTANCE:" + String(boatDistance) + "|";
    statusMsg += "BRIDGE_MOVEMENT_DISTANCE:" + String(bridgeMovementDistance) + "|";
    statusMsg += "BOAT_CLEARANCE_DISTANCE:" + String(boatClearanceDistance) + "|";
    statusMsg += "ROAD_LIGHT:" + String(lightColourToString(currentRoadTrafficLight)) + "|";
    statusMsg += "BOAT_LIGHT:" + String(lightColourToString(currentBoatTrafficLight)) + "|";
    statusMsg += "BRIDGE_LIGHT:" + String(bridgeLightStateToString(currentBridgeLightState)) + "|";
    statusMsg += "MANUAL_BRIDGE_LIGHTS:" + String(manualBridgeLights ? "YES" : "NO") + "|";
    statusMsg += "SEQUENCE:" + String(sequenceStateToString(currentSequenceState)) + "|";
    statusMsg += "MOVEMENT_STATE:" + String(movementStateToString(currentMovementState));

    // Add queue info if in override mode
    if (currentOperationalMode == OVERRIDE) {
      UBaseType_t queuedCommands = uxQueueMessagesWaiting(pendingOverrideQueue);
      statusMsg += "|QUEUE:" + String(queuedCommands);
      statusMsg += "|EXECUTING:" + String(isBridgeMovementExecutingInOverride ? "YES" : "NO");
    }

    // Send the status update and release the mutex
    sendUpdate(statusMsg);
    xSemaphoreGive(stateMutex);
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22

private void handleStatusUpdate(String message) {
  String content = message.substring(message.indexOf(":") + 1).trim();
  String[] parts = content.split("\\|");
  
  for (String part : parts) {
    String[] keyValue = part.split(":");
    if (keyValue.length == 2) {
      String key = keyValue[0].trim();
      String value = keyValue[1].trim();
      
      switch (key) {
        case "MODE":
          userInterface.updateMode(value);
          break;
        case "BRIDGE":
          userInterface.updateBridgeState(value);
          break;
        // ... Etc
      }
    }
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

/* The main loop */
void loop() {
  // Listen for incoming UDP packets
  int packetSize = udp.parsePacket();

  // If there is a packet then accordingly process the command
  if (packetSize) {
    // Buffer to hold incoming packet
    char packetBuffer[255];
    // Set all bytes at the buffer in memory to 0
    memset(packetBuffer, 0, sizeof(packetBuffer));
    // Read the packet into the buffer
    int bytesRead = udp.read(packetBuffer, packetSize);
    // Null-terminate at the exact length received
    packetBuffer[bytesRead] = '\0';
    // Convert buffer to string
    String cmd = String(packetBuffer);
    // Remove any extra whitespace
    cmd.trim();

    // Remove null characters if they exist
    cmd.remove(cmd.indexOf(0x00));
    for (int i = 0; i < cmd.length(); i++) {
      if (cmd[i] < 32 || cmd[i] > 126) {
        // Remove non-printable character
        cmd.remove(i, 1);
        // Adjust index after removal
        i--;
      }
    }

    Serial.println("INFO: Command received: " + cmd);

    // ... Rest of the message received logic
  }

  // ... Rest of the main loop
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

public class Heartbeat extends Thread {
    private Send heartBeatSendObject;
    private Timer heartBeatTimer;
    private Gui userInterface;

    // Heartbeat interval in milliseconds (2 seconds)
    private static final long HEARTBEAT_INTERVAL = 2000;
    // Initial delay before first heartbeat (1 second)
    private static final long INITIAL_DELAY = 1000;

    Heartbeat(Send heartbeatObject, Gui userInterface) {
        this.heartBeatSendObject = heartbeatObject;
        this.userInterface = userInterface;
        // Make it a daemon thread
        this.heartBeatTimer = new Timer(true);
    }

    @Override
    public void run() {
        // scheduleAtFixedRate for repeating task
        heartBeatTimer.scheduleAtFixedRate(new TimerTask() {
            @Override
            public void run() {
                try {
                    heartBeatSendObject.sendMessage("heartbeat");
                    if (userInterface != null) {
                        userInterface.updateMessageLog(
                                "SENT: heartbeat");
                    }
                } catch (Exception e) {
                    System.err.println("Error sending heartbeat: " + e.getMessage());
                }
            }
        }, INITIAL_DELAY, HEARTBEAT_INTERVAL);
    }

    public void stopHeartbeat() {
        if (heartBeatTimer != null) {
            heartBeatTimer.cancel();
            System.out.println("Heartbeat stopped");
        }
    }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

// Check heartbeat timeout
  if (xSemaphoreTake(stateMutex, 100 / portTICK_PERIOD_MS)) {
    // Check communication with Java program has been lost
    bool heartbeatTimedOut = (millis() - lastHeartbeatMessage > HEARTBEAT_INTERVAL);
    // Communication just got lost
    if (heartbeatTimedOut && communicationWithJavaIsAlive) {
      communicationWithJavaIsAlive = false;
      Serial.println("SYSTEM_UPDATE: Lost communication with Java program");

      if (currentOperationalMode == OVERRIDE) {
        // Clear override queue
        int cleared = clearOverrideQueue();
        if (cleared > 0) {
          Serial.print("INFO: Cleared ");
          Serial.print(cleared);
          Serial.println(" queued override commands");
        }

        currentOperationalMode = AUTOMATIC;

        // Only reset state if not currently executing
        if (!isBridgeMovementExecutingInOverride) {
          currentSequenceState = IDLE;
          currentMovementState = STATE_ONE;
          Serial.println("SYSTEM_UPDATE: Reset to IDLE state (no execution in progress)");
        } else {
          // Check if execution has been going too long
          unsigned long executionTime = millis() - overrideExecutionStartTime;
          const unsigned long OVERRIDE_EXECUTION_TIMEOUT = 60000;  // 60 seconds

          if (executionTime > OVERRIDE_EXECUTION_TIMEOUT) {
            Serial.println("ERROR: Override execution timeout - forcing DIAGNOSTIC");
            currentSequenceState = DIAGNOSTIC;
            isBridgeMovementExecutingInOverride = false;
            setRoadTrafficLights(ALL);
            setBoatTrafficLights(ALL);
          } else {
            Serial.print("INFO: Allowing operation to complete (");
            Serial.print((OVERRIDE_EXECUTION_TIMEOUT - executionTime) / 1000);
            Serial.print("s timeout remaining, state: ");
            Serial.print(sequenceStateToString(currentSequenceState));
            Serial.println(")");
          }
        }

        manualBridgeLights = false;

        // Cancel any pending mode changes
        pendingModeChange.pending = false;

        Serial.println("SYSTEM_UPDATE: Switched to automatic mode due to communication loss");
        sendUpdate("SYSTEM_UPDATE: communication_lost");
      }
    }
    xSemaphoreGive(stateMutex);
  }

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

// Check for what the weight is on the bridge through OP Amp
long checkBridgeWeight() {
  Serial.println("INFO: Performing bridge weight check");

  // Rotate the motor in one direction (600 microseconds, clockwise) briefly
  int dutyCycleCW = (600 / 20000.0) * 255;
  ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleCW);

  // Wait for current to stabilise
  delay(50);

  // Read the current draw (proportional to weight)
  long weightReading = readAdcSensorValue(BRIDGE_WEIGHT);
  delay(25);
  long weightReading2 = readAdcSensorValue(BRIDGE_WEIGHT);
  delay(25);
  long weightReading3 = readAdcSensorValue(BRIDGE_WEIGHT);

  // Get the average
  long averageWeight = (weightReading + weightReading2 + weightReading3) / 3;

  // Stop motor immediately
  int dutyCycleStop = (1500 / 20000.0) * 255;
  ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleStop);

  Serial.print("INFO: Weight check ADC reading is: ");
  Serial.println(averageWeight);

  return averageWeight;
#endif
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

long weightReading = checkBridgeWeight();

// Send weight reading to Java
sendUpdate("WEIGHT_CHECK: " + String(weightReading));

// Re-acquire lock
xSemaphoreTake(stateMutex, portMAX_DELAY);

if (weightReading > WEIGHT_SAFETY_MARGIN) {
  Serial.print("INFO: Weight on bridge is overloaded, re-closing bridge: ");
  Serial.println(weightReading);

  int dutyCycleACW = (2000 / 20000.0) * 255;
  ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleACW);
  xSemaphoreGive(stateMutex);

  // Ensure return to close
  delay(150);

  xSemaphoreTake(stateMutex, portMAX_DELAY);

  int dutyCycleStop = (1500 / 20000.0) * 255;
  ledcWrite(PWM_CHANNEL_BRIDGE, dutyCycleStop);

  sendUpdate("SYSTEM_UPDATE: bridge_overloaded");

  currentBridgeState = CLOSED;
  xSemaphoreGive(stateMutex);
  // Don't proceed with opening
  continue;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

const int BRIDGE_BOAT_DETECTED_WHILE_MOVING = 15;

while (bridgeDistance < BRIDGE_CLOSED_DISTANCE) {
  // Timeout check
  if (millis() - operationStartTime > BRIDGE_TIMEOUT) {
    timeoutOccurred = true;
    break;
  }
  
  // Clearance check
  long distanceUnderBridge = readUltrasonicDistanceBoatClearance();
  if (isValidSensorReading(distanceUnderBridge) && 
      distanceUnderBridge < BRIDGE_BOAT_DETECTED_WHILE_MOVING) {
    Serial.println("ERROR: Detected boat while closing bridge");
    boatsDetectedDuringClosing = true;
    // Stop moving the bridge
    break;
  }
  
  // ... Rest of the close bridge logic
}

// After the loop handle the boat detection
if (boatsDetectedDuringClosing) {
  currentBridgeState = OPEN;
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19

else if (cmd.startsWith("override_mode")) {
    // Move to override
    // Mutex already held at this point so read the sensor values
    long roadDistance = readUltrasonicDistanceRoadTraffic();
    long boatDistance = readUltrasonicDistanceBoatTraffic();

    bool roadTrafficDetected = isValidSensorReading(roadDistance) && roadDistance < CARS_WAITING_DISTANCE * 0.7;
    bool boatTrafficDetected = isValidSensorReading(boatDistance) && boatDistance < BOATS_WAITING_DISTANCE * 0.7;

    if (roadTrafficDetected || boatTrafficDetected) {
      Serial.println("ERROR: Override mode denied - traffic present");
      sendUpdate("ERROR: override_denied_traffic_present");

      // Don't queue the request
      pendingModeChange.pending = false;
    }

    // ,, Rest of the override mode command logic
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

bool isSafeToSwitchMode() {
  return ((currentSequenceState == IDLE || 
           currentSequenceState == BOATS_PASSING || 
           currentSequenceState == CARS_PASSING) &&
          !isBridgeMovementExecutingInOverride);
}

// In main loop:
if (pendingModeChange.pending) {
  unsigned long elapsed = millis() - pendingModeChange.requestTime;
  
  if (isSafeToSwitchMode()) {
    // Execute the queued mode change
    currentOperationalMode = pendingModeChange.requestedMode;
    currentSequenceState = IDLE;
    clearOverrideQueue();
    pendingModeChange.pending = false;
  }
  else if (elapsed > 60000) {
    // Timeout after 60 seconds
    Serial.println("ERROR: Mode change request timed out");
    pendingModeChange.pending = false;
  }
}

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

case DIAGNOSTIC:
  Serial.println("SYSTEM_UPDATE: System is in DIAGNOSTIC mode");
  setRoadTrafficLights(ALL);
  setBoatTrafficLights(ALL);
  
  long bridgeDistance = readUltrasonicDistanceBridgeMovement();
  
  if (isValidSensorReading(bridgeDistance)) {
    // Double tolerance for recovery (±4cm instead of ±2cm)
    if (abs(bridgeDistance - BRIDGE_OPEN_DISTANCE) < TOLERANCE * 2) {
      // Bridge appears open
      currentBridgeState = OPEN;
      currentSequenceState = BOATS_PASSING;
      changeGateStateAsync(CLOSED);
      vTaskDelay(GATE_TRANSITION_INTERVAL / portTICK_PERIOD_MS);
      setBoatTrafficLights(GREEN);
      setRoadTrafficLights(RED);
      sequenceStartTime = millis();
      Serial.println("SYSTEM_UPDATE: Recovered, bridge is OPEN");
      return;
    } 
    else if (abs(bridgeDistance - BRIDGE_CLOSED_DISTANCE) < TOLERANCE * 2) {
      // Bridge appears closed
      currentBridgeState = CLOSED;
      currentSequenceState = CARS_PASSING;
      changeGateStateAsync(OPEN);
      vTaskDelay(GATE_TRANSITION_INTERVAL / portTICK_PERIOD_MS);
      setBoatTrafficLights(RED);
      setRoadTrafficLights(GREEN);
      Serial.println("SYSTEM_UPDATE: Recovered, bridge is CLOSED");
      return;
    }
  }
  
  // Unable to recover
  Serial.println("ERROR: Bridge position unknown, restart required");
  restartRequired = true;
  break;

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14

const unsigned long BRIDGE_TIMEOUT = 30000;

while (bridgeDistance > BRIDGE_OPEN_DISTANCE) {
  if (millis() - operationStartTime > BRIDGE_TIMEOUT) {
    timeoutOccurred = true;
    break;
  }
  // ... movement logic
}

if (timeoutOccurred) {
  // Triggers diagnostic mode
  currentBridgeState = UNKNOWN;
}