main.ino

#define LEFT_LED_PIN 11
#define RIGHT_LED_PIN 10

#define BEACON_TRIG 9

#define LEFT_ECHO 2
#define RIGHT_ECHO 3

#define true 1
#define false 0

int mil = 0;
int current_seconds = 0;
int seconds = 0;
bool status = false;

void LEFTOn() {
    digitalWrite(LEFT_LED_PIN, HIGH);
}

void LEFTOff() {
    digitalWrite(LEFT_LED_PIN, LOW);
}

void RIGHTOn() {
    digitalWrite(RIGHT_LED_PIN, HIGH);
}

void RIGHTOff() {
    digitalWrite(RIGHT_LED_PIN, LOW);
}

void setup() {
    Serial.begin(9600);
        
    Serial.println("started");

    pinMode(LEFT_LED_PIN, OUTPUT);
    pinMode(RIGHT_LED_PIN, OUTPUT);
    pinMode(BEACON_TRIG, OUTPUT);
    pinMode(LEFT_ECHO, INPUT);
    pinMode(RIGHT_ECHO, INPUT);

    attachInterrupt(1, onLeftEcho, CHANGE);
    attachInterrupt(2, onRightEcho, CHANGE);

    digitalWrite(BEACON_TRIG, LOW);

    interrupts();
}

void loop() {
    mil = millis();

    current_seconds = ((mil % 1000) / 100);

    if(seconds != current_seconds) {
        Serial.println("s");
        seconds = current_seconds;
        digitalWrite(BEACON_TRIG, HIGH);
        
        interrupts();
    }
}

void onRightEcho() {
    noInterrupts();
    Serial.println("r");
    LEFTOff();
    RIGHTOn();
    digitalWrite(BEACON_TRIG, LOW);
}

void onLeftEcho() {
    noInterrupts();
    Serial.println("l");
    RIGHTOff();
    LEFTOn();
    digitalWrite(BEACON_TRIG, LOW);
}