makeathon.c

#define SENSOR_N A0
#define SENSOR_E A1
#define SENSOR_S A2
#define SENSOR_W A3

#define LED_N 4 // right
#define LED_E 3
#define LED_S 5
#define LED_W 2

#define IDX_N 0
#define IDX_E 1
#define IDX_S 2
#define IDX_W 3

#define ADC_CUTOFF 400 // who knows
struct vehicle_arr {
  uint8_t pos;
  unsigned long timestamp;
};

struct arrive_ring {
  struct vehicle_arr ring[4];
  uint8_t front; // pop
  uint8_t back; // push
  bool full;
};

struct arrive_ring arrive_ring = {0};
uint8_t occupied = 0;

bool ring_full(void) {
  return arrive_ring.back == arrive_ring.front && arrive_ring.full;
}

bool ring_empty(void) {
  return arrive_ring.back == arrive_ring.front && !arrive_ring.full;
}

int ring_size(void) {
  if(ring_full) return 4;
  return (unsigned)(arrive_ring.back - arrive_ring.front) % 4;
}

bool ring_push(struct vehicle_arr *val) {
  if(ring_full()) return false;
  arrive_ring.ring[arrive_ring.back] = *val;
  arrive_ring.back = (arrive_ring.back + 1) % 4;
  if(arrive_ring.back == arrive_ring.front) arrive_ring.full = true;
}

bool ring_pop(struct vehicle_arr *val) {
  if(ring_empty()) return false;
  if(ring_full()) arrive_ring.full = false;
  struct vehicle_arr v = arrive_ring.ring[arrive_ring.front];
  if(val != NULL) *val = v;
  occupied &= ~(1<<v.pos);
  arrive_ring.front = (arrive_ring.front + 1) % 4;
}

void arrive(uint8_t idx) {
  if(occupied & (1<<idx)) return;
  occupied |= 1<<idx;
  struct vehicle_arr a;
  a.timestamp = millis();
  a.pos = idx;
  ring_push(&a);
}

void setup() {
  // put your setup code here, to run once:
  //attachInterrupt(digitalPinToInterrupt(LED_N), led_isr_n, RISING);
  //attachInterrupt(digitalPinToInterrupt(LED_E), led_isr_e, RISING);
  //attachInterrupt(digitalPinToInterrupt(LED_S), led_isr_s, RISING);
  //attachInterrupt(digitalPinToInterrupt(LED_W), led_isr_w, RISING);
  pinMode(LED_N, OUTPUT);
  pinMode(LED_E, OUTPUT);
  pinMode(LED_S, OUTPUT);
  pinMode(LED_W, OUTPUT);

  pinMode(SENSOR_N, INPUT);
  pinMode(SENSOR_E, INPUT);
  pinMode(SENSOR_S, INPUT);
  pinMode(SENSOR_W, INPUT);

  Serial.begin(115200);
}

void light_set(int idx, bool value) {
  switch(idx) {
    case IDX_N:
    digitalWrite(LED_N, value);
    break;
    case IDX_E:
    digitalWrite(LED_E, value);
    break;
    case IDX_S:
    digitalWrite(LED_S, value);
    break;
    case IDX_W:
    digitalWrite(LED_W, value);
    break;
  }
}

bool car_present(int idx) {
  switch(idx) {
    case IDX_N: return analogRead(SENSOR_N) < ADC_CUTOFF;
    case IDX_E: return analogRead(SENSOR_E) < ADC_CUTOFF;
    case IDX_S: return analogRead(SENSOR_S) < ADC_CUTOFF;
    case IDX_W: return analogRead(SENSOR_W) < ADC_CUTOFF;
  }
}

enum intersection_state {
  EMPTY,
  WAITING,
  CAR_MOVE_WAIT,
  CAR_MOVING,
  ONE_CAR_WAIT,
};

enum intersection_state state;
unsigned long mill = 0;
void loop() {
  if(car_present(IDX_N)) arrive(IDX_N);
  if(car_present(IDX_E)) arrive(IDX_E);
  if(car_present(IDX_S)) arrive(IDX_S);
  if(car_present(IDX_W)) arrive(IDX_W);
  struct vehicle_arr *a = arrive_ring.ring + arrive_ring.front;
  //Serial.println(analogRead(SENSOR_N));
  //Serial.println(state);
  Serial.print(arrive_ring.back);
  Serial.print(" ");
  Serial.print(arrive_ring.front);
  Serial.print(" - ");
  Serial.println(arrive_ring.full);
  switch(state) {
    case EMPTY:
    Serial.println("empty");
    if(!ring_empty()){
      state = WAITING;
      mill = millis();
      Serial.print("not empty anymore ");
      Serial.print(millis());
      Serial.print(" ");
      Serial.println(mill);
    }
    break;
    case WAITING:
    Serial.println("waiting");
    if(ring_empty()) {
      state = EMPTY;
      break;
    }
    if((millis() - mill) > 400) {
      //Serial.print(millis());
      //Serial.print(" ");
      //Serial.println(mill);
      /*if(ring_size() == 1) {
        state = ONE_CAR_WAIT;
        mill = millis();
        break;
      }*/
      mill = millis();
      state = CAR_MOVE_WAIT;
      light_set(a->pos, true);
      break;
    }
    break;
    case CAR_MOVE_WAIT:
    Serial.println("car_move_wait");
    if(!car_present(a->pos)) {
      state = CAR_MOVING;
      light_set(a->pos, false);
      mill = millis();
    }
    break;
    case CAR_MOVING:
    Serial.println("car_moving");
    if(millis() - mill > 680) {
      ring_pop(NULL);
      state = EMPTY;
      break;
    }
    break;
    /*case ONE_CAR_WAIT:
    Serial.println("one_car_wait");
    if(millis() - mill > 2000) {
      ring_pop(NULL);
      state = EMPTY;
    }
    break;*/
  }
}