#include<ESP32Servo.h>

#define pulAmarillo 4
#define pulRojo 5

#define pinServo 1
Servo myservo;

#define ledRojo 39
#define ledVerde 38
#define ledAzul 37

int estado = 1;
const int anguloIni = 0;
volatile bool btnApresionado = false;
volatile bool btnRpresionado = false;
unsigned long tiempoEmergencia = 0;
void ARDUINO_ISR_ATTR isr_pulA(void){
   if (btnApresionado) {
      btnApresionado = !btnApresionado;
      delay(10000);
    }
}
void ARDUINO_ISR_ATTR isr_pulR(void){
  btnRpresionado = !btnRpresionado;
  myservo.write(anguloIni);
  digitalWrite(ledRojo, HIGH);
  digitalWrite(ledVerde, LOW);
  digitalWrite(ledAzul, LOW);
}
void setup() {
  
  pinMode(ledRojo, OUTPUT);
  pinMode(ledVerde, OUTPUT);
  pinMode(ledAzul, OUTPUT);
  pinMode(pulAmarillo, INPUT_PULLUP);
  pinMode(pulRojo, INPUT_PULLUP);

  digitalWrite(ledRojo, LOW);
  digitalWrite(ledVerde, LOW);
  digitalWrite(ledAzul, LOW);

  attachInterrupt(pulAmarillo, isr_pulA, FALLING);
  attachInterrupt(pulRojo, isr_pulR, FALLING);
  myservo.attach(pinServo);
  myservo.write(anguloIni);
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("20217398-Lab10");
}

void loop() {
  if(btnRpresionado==false){
    delay(1000);
    if(estado==1){
      digitalWrite(ledRojo, LOW);
      digitalWrite(ledVerde, LOW);
      digitalWrite(ledAzul, HIGH);
      myservo.write(180);
      estado = 2;
    }else if(estado==2){
      digitalWrite(ledRojo, LOW);
      digitalWrite(ledVerde, HIGH);
      digitalWrite(ledAzul, LOW);
      myservo.write(90);
      estado = 3;
    }else if(estado==3){
      digitalWrite(ledRojo, HIGH);
      digitalWrite(ledVerde, LOW);
      digitalWrite(ledAzul, HIGH);
      myservo.write(45);
      estado = 1;
    }
    delay(1000);
  }else{
    delay(5000);
    btnRpresionado=false;
  }
  delay(10); // this speeds up the simulation
}