#include <Servo.h>

// ovde se vezu ulazi sa releja,
// znaci stavite 10 k prema zemlji sa tih pinova
// i takodje povezete releje na njih, bilo bi dobro
// postaviti mali kondenzator keramicki izmedju ulaza i GND
#define otvoriRampu 2
#define zatvoriRampu 3

// obe zelene na ovaj pin, mislim da ne treba tranzistor
#define semaforZelena 4

// obe crvene na ovaj pin
#define semaforCrvena 5

// definisemo pinove servo motora i njihove objekte
#define servoPin1 9
#define servoPin2 10

Servo servo1, servo2;

// definisimo uglove za zatvoreno, otvoreno ( prilagodite po potrebi ), i trenutni ugao
#define ugaoZatvoreno 0
#define ugaoOtvoreno 90
byte trenutniUgao;

void setup() {
  // postavimo ulaze izlaze
  pinMode(otvoriRampu, INPUT);
  pinMode(zatvoriRampu, INPUT);
  pinMode(semaforZelena, OUTPUT);
  pinMode(semaforCrvena, OUTPUT);

  // zakacimo servo motore i vratimo ih u pocetne pozicije tj na ugao od 90
  // stepeni, podesite polugice na pravi polozaj kada vidite da je odradio ovaj dio
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  servo1.write(ugaoOtvoreno);
  servo2.write(ugaoOtvoreno);
  trenutniUgao = ugaoOtvoreno;
  // postavimo stanja sijalica
  digitalWrite(semaforZelena, 1);
  digitalWrite(semaforCrvena, 0);


}

void loop() {

  // provjerimo da li dobijamo signal na ulazu za zatvaranje rampe
  if(digitalRead(zatvoriRampu)){
    // posto koristimo induktivne senzore postoji mogucnos da ce ova
    // petlja biti zakacena vise puta, tako da cemo ovo samo uraditi ako
    // mjenjamo ugao semafora..
    if(trenutniUgao != ugaoZatvoreno){
      // promjenimo semafor
      digitalWrite(semaforCrvena, 1);
      digitalWrite(semaforZelena, 0);
      // postavimo servo u zatvorenu poziciju
      servo1.write(ugaoZatvoreno);
      servo2.write(ugaoZatvoreno);
      trenutniUgao = ugaoZatvoreno;
    }

  }
  // provjerimo da li dobijamo signal na ulazu za otvaranje rampe
  if(digitalRead(otvoriRampu)){
     // posto koristimo induktivne senzore postoji mogucnos da ce ova
    // petlja biti zakacena vise puta, tako da cemo ovo samo uraditi ako
    // mjenjamo ugao semafora..
    if(trenutniUgao != ugaoOtvoreno){
      // promjenimo semafor
      digitalWrite(semaforCrvena, 0);
      digitalWrite(semaforZelena, 1);
      // postavimo servo u zatvorenu poziciju
      servo1.write(ugaoOtvoreno);
      servo2.write(ugaoOtvoreno);
      trenutniUgao = ugaoOtvoreno;
    }
  }
  // neka program ima mali odmor
  delay(50);
}