RTS_SignalController

/* Main.ino file generated by New Project wizard
 *
 * Created:   Wed Nov 13 2019
 * Processor: Arduino Uno
 * Compiler:  Arduino AVR (Proteus)
 */

#include <ArduinoSTL.h>
#include <vector>

#define T1 12000
#define T2 7000
#define T3 18000
#define T4 9000
#define T5 500
#define T6 35000
#define T7 25000
#define T8 30000
#define T9 4000

#define LEFT_LED_PIN 6
#define MIDDLE_LED_PIN 5
#define RIGHT_LED_PIN 4
#define BOTTOM_LED_PIN 7
#define AUX_LED_PIN 8
#define DEBUG_LED_PIN 3

#define Line16_BTN 11
#define Line15_BTN 10
#define Line26_BTN 9
#define Start_BTN 12

unsigned long state_timer = 0;
unsigned long finishAt = 0;
unsigned long hyperstate_timer = 0;

typedef enum State {
  S0_IDLE, S1_WAIT, S2, S3, S4_L, S5_LL, S6_LF, S7_F, S8_FF,
  S9_FR, S10_R, S11_RR, S12_LR ,S13_INHIBIT_S, S14_INHIBIT_U, S15_ALL
};

enum Mode {u, lf, fr, lr, all};

enum Line {
  L1 = 1, L2, L3, L4, L5, L6, L7, L8, L9, L10, L11, L12, L13, L14, L15, L16,
  L17, L18, L19, L20, L21, L22, L23, L24, L25, L26, L27, L28, L29, L30, L31
};

typedef struct {
    uint8_t mode:3;
    uint8_t PhaseExt_L:1; // XL
    uint8_t PhaseExt_F:1; // XF
    uint8_t PhaseExt_R:1; // XR
} input_config;

uint8_t Start = 0;

input_config sig = {fr, 1, 1, 1};

const std::vector <uint16_t> L_lines {L16};
const std::vector <uint16_t> LF_lines {L16, L15};
const std::vector <uint16_t> F_lines {L15};
const std::vector <uint16_t> All_lines {L15, L16, L26};
const std::vector <uint16_t> FR_lines {L15, L26};
const std::vector <uint16_t> R_lines {L26};
const std::vector <uint16_t> LR_lines {L16, L26};

void timed_finite_state_machine_run(input_config mode, uint16_t line);
uint8_t check_line(uint16_t line, const std::vector <uint16_t> &lines_set);
void read_transit_line();

volatile State state = S0_IDLE;
volatile uint16_t line = 0;
uint8_t isTransitPresent = 0;
uint8_t pinToAnalize = 0;


void setup() {
  pinMode(LEFT_LED_PIN, OUTPUT);
  pinMode(MIDDLE_LED_PIN, OUTPUT);
  pinMode(RIGHT_LED_PIN, OUTPUT);
  pinMode(BOTTOM_LED_PIN, OUTPUT);
  pinMode(AUX_LED_PIN, OUTPUT);
  pinMode(DEBUG_LED_PIN, OUTPUT);
  pinMode(Line15_BTN, INPUT);
  pinMode(Line16_BTN, INPUT);
  pinMode(Line26_BTN, INPUT);
  pinMode(Start_BTN, INPUT);

  state_timer = millis();
  Serial.begin(9600);
}

void loop() {
  Serial.print("Current state: ");
  Serial.println(state, DEC);
  timed_finite_state_machine_run(sig, line);

  delay(10);

  if (digitalRead(Start_BTN) == LOW) {
    delay(10);
    if (digitalRead(Start_BTN) == LOW) {
      Start = 1;
    }
  }
  else {
    Start = 0;
  }

  delay(10);

  digitalWrite(LEFT_LED_PIN, (
  state == S1_WAIT || state == S2 || state == S3 || state == S4_L || state == S5_LL || state == S6_LF ||
  state == S12_LR || state == S13_INHIBIT_S || state == S14_INHIBIT_U || state == S15_ALL
  ) ? HIGH : LOW);

  delay(10);

  digitalWrite(MIDDLE_LED_PIN, (
  state == S1_WAIT || state == S2 || state == S3 || state == S6_LF || state == S7_F || state == S8_FF ||
  state == S9_FR || state == S13_INHIBIT_S || state == S14_INHIBIT_U || state == S15_ALL
  ) ? HIGH : LOW);

  delay(10);

  digitalWrite(RIGHT_LED_PIN, (
  state == S1_WAIT || state == S2 || state == S3 || state == S9_FR || state == S10_R || state == S11_RR ||
  state == S12_LR || state == S13_INHIBIT_S || state == S14_INHIBIT_U || state == S15_ALL
  ) ? HIGH : LOW);

  delay(10);

  digitalWrite(BOTTOM_LED_PIN, (
  state == S2 || state == S4_L || state == S5_LL || state == S6_LF || state == S7_F || state == S8_FF ||
  state == S9_FR || state == S10_R || state == S11_RR || state == S12_LR || state == S15_ALL
  ) ? HIGH : LOW);

  delay(10);

  digitalWrite(AUX_LED_PIN, state == S0_IDLE ? HIGH : LOW);

  delay(10);

  read_transit_line();

  delay(10);
}

void timed_finite_state_machine_run(const input_config config, const uint16_t line) {
  switch(state)
  {
    case S0_IDLE:
      if (Start == 0) {
        state = S0_IDLE;
      }
      else if (config.mode == all) {
        state = S14_INHIBIT_U;
      }
      else {
        state = S1_WAIT;
      }
      break;

    case S1_WAIT:
      if (Start == 0) {
        state = S0_IDLE;
      }
      else if (check_line(line, All_lines)) {
        state = S2;
        finishAt = millis() + T9;
        hyperstate_timer = millis();
      }
      else {
        state = S1_WAIT;
      }
      state_timer = millis();
      break;

    case S2:
      if (millis() < finishAt) {
        unsigned long currentMillis = millis();
        if (currentMillis - hyperstate_timer >= T5) {
          hyperstate_timer = currentMillis;
          state = S3;
        }
      }

      if (millis() - state_timer >= T9) {
        if (check_line(line, L_lines)&((config.mode == fr)|(config.mode == u))) {
          state = S4_L;
        }
        else if (check_line(line, LF_lines)&(config.mode == lf)) {
          state = S6_LF;
        }
        else if (check_line(line, F_lines)&((config.mode == lr)||(config.mode == u))) {
          state = S7_F;
        }
        else if (check_line(line, FR_lines)&(config.mode == fr)) {
          state = S9_FR;
        }
        else if (check_line(line, R_lines)&((config.mode == lf)|(config.mode == u))) {
          state = S10_R;
        }
        else if (check_line(line, LR_lines)&(config.mode == lr)) {
          state = S12_LR;
        }
        else {
          state = S1_WAIT;
        }
        state_timer = millis();
      }
      break;

    case S3:
      if (millis() < finishAt) {
        unsigned long currentMillis = millis();
        if (currentMillis - hyperstate_timer >= T5) {
          hyperstate_timer = currentMillis;
          state = S2;
        }
      }

      if (millis() - state_timer >= T9) {
        if (check_line(line, L_lines)&((config.mode == fr)|(config.mode == u))) {
          state = S4_L;
        }
        else if (check_line(line, LF_lines)&(config.mode == lf)) {
          state = S6_LF;
        }
        else if (check_line(line, F_lines)&((config.mode == lr)||(config.mode == u))) {
          state = S7_F;
        }
        else if (check_line(line, FR_lines)&(config.mode == fr)) {
          state = S9_FR;
        }
        else if (check_line(line, R_lines)&((config.mode == lf)|(config.mode == u))) {
          state = S10_R;
        }
        else if (check_line(line, LR_lines)&(config.mode == lr)) {
          state = S12_LR;
        }
        else {
          state = S1_WAIT;
        }
        state_timer = millis();
      }
      break;

    case S4_L:
      if (millis() - state_timer >= T3) {
        if (check_line(line, L_lines)&config.PhaseExt_L) {
          state = S5_LL;
        }
        else {
          state = S13_INHIBIT_S;
        }
      state_timer = millis();  // re-initialise state_timer for next period
      }
      break;

    case S5_LL:
      if (millis() - state_timer >= T4) {
        if (config.mode == all) {
          state = S14_INHIBIT_U;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S6_LF:
      if (millis() - state_timer >= T3) {
        if (check_line(line, L_lines)&config.PhaseExt_L) {
          state = S5_LL;
        }
        else if (check_line(line, F_lines)&config.PhaseExt_F) {
          state = S8_FF;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S7_F:
      if (millis() - state_timer >= T1) {
        if (check_line(line, F_lines)&config.PhaseExt_F) {
          state = S8_FF;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S8_FF:
      if (millis() - state_timer >= T2) {
        if(config.mode == all) {
          state = S14_INHIBIT_U;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S9_FR:
      if (millis() - state_timer >= T3) {
        if (check_line(line, F_lines)&config.PhaseExt_F) {
          state = S8_FF;
        }
        else if (check_line(line, R_lines)&config.PhaseExt_R) {
          state = S11_RR;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S10_R:
      if (millis() - state_timer >= T3) {
        if (check_line(line, R_lines)&config.PhaseExt_R) {
          state = S11_RR;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S11_RR:
      if (millis() - state_timer >= T4) {
        if (config.mode == all) {
          state = S14_INHIBIT_U;
        }
        else {
          state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S12_LR:
      if (millis() - state_timer >= T3) {
        if (check_line(line, L_lines)&config.PhaseExt_L) {
          state = S5_LL;
        }
        else if (check_line(line, R_lines)&config.PhaseExt_R) {
          state = S11_RR;
        }
        else {
           state = S13_INHIBIT_S;
        }
        state_timer = millis();
      }
      break;

    case S13_INHIBIT_S:
      if (millis() - state_timer >= T8) {
        state = S1_WAIT;
        state_timer = millis();
      }
      break;

    case S14_INHIBIT_U:
      if (millis() - state_timer >= T6) {
        if (Start == 0) {
          state = S0_IDLE;
        }
        else {
          state = S15_ALL;
        }
        state_timer = millis();
      }
      break;

    case S15_ALL:
      if (millis() - state_timer >= T7) {
        if (check_line(line, L_lines)&config.PhaseExt_L) {
          state = S5_LL;
        }
        else if (check_line(line, F_lines)&config.PhaseExt_F) {
          state = S8_FF;
        }
        else if (check_line(line, R_lines)&config.PhaseExt_R) {
          state = S11_RR;
        }
        else {
          state = S14_INHIBIT_U;
        }
        state_timer = millis();
      }
      break;

    default:
      state = S0_IDLE;
  }
}

void read_transit_line() {
  if(!isTransitPresent) {
    if (digitalRead(Line15_BTN) == LOW) {
      Serial.println("L15 here");
      line = L15;
      pinToAnalize = Line15_BTN;
      isTransitPresent = 1;
    }

    if (digitalRead(Line16_BTN) == LOW) {
      Serial.println("L16 here");
      line = L16;
      pinToAnalize = Line16_BTN;
      isTransitPresent = 1;
    }

    if (digitalRead(Line26_BTN) == LOW) {
      Serial.println("L26 here");
      line = L26;
      pinToAnalize = Line26_BTN;
      isTransitPresent = 1;
    }
  }
  else {
    if (digitalRead(pinToAnalize) == HIGH) {
      isTransitPresent = 0;
    }
  }
}

uint8_t check_line(const uint16_t line, const std::vector <uint16_t> &lines_set) {
   for (size_t i = 0; i < lines_set.size(); ++i) {
      if (line == lines_set[i]) {
         return 1;
      }
   }
   return 0;
}