#include <ESP32Servo.h>

const int pinSensorIzquierda = 7;
const int pinSensorDerecha = 15;
const int pinServomotorIzquierda = 1;
const int pinServomotorDerecha = 2;

Servo ServoIzquierda;
Servo ServoDerecha;

volatile bool Movimiento;
volatile bool Interrupcion;
int AnguloPosicion;

void setup() {
  //Puerto serial
  Serial.begin(115200);
  //Sensores
  pinMode(pinSensorIzquierda, INPUT);
  pinMode(pinServomotorDerecha, INPUT);
  //Servo
  ServoIzquierda.attach(pinServomotorIzquierda);
  ServoDerecha.attach(pinServomotorDerecha);
  //Interrupciones
  attachInterrupt(pinSensorIzquierda,ISR_SensorIzquierda,ONHIGH);
  attachInterrupt(pinSensorDerecha,ISR_SensorDerecha,ONHIGH);
  Movimiento = false;
  Interrupcion = false;
  //Se cierra el portón
  CerrarPorton();
}

void loop() {
  if(Movimiento){
    Movimiento = false;
    AbrirPorton();
    delay(5000);
    CerrarPorton();
  }
  delay(10);
}

void ARDUINO_ISR_ATTR ISR_SensorIzquierda(){
  if(!Movimiento){
    Movimiento = true;
  }else{
    Interrupcion = true;
  }
}

void ARDUINO_ISR_ATTR ISR_SensorDerecha(){
  if(!Movimiento){
    Movimiento = true;
  }else{
    Interrupcion = true;
  }
}

void AbrirPorton(){
  Serial.println("Abriendo portón");
  AnguloPosicion = 0;
  while(AnguloPosicion <= 90){ //En el bucle n se mueve al ángulo n desde el n-1
    ServoIzquierda.write(AnguloPosicion);
    ServoDerecha.write(180-AnguloPosicion);
    delay(10);
    AnguloPosicion++;
  }
}

void CerrarPorton(){
  Serial.println("Cerrando portón");
  AnguloPosicion = 90;
  while(AnguloPosicion>=0){
    if(Interrupcion){
      Interrupcion = false;
      AbrirPorton();
      delay(5000);
      CerrarPorton();
    }else{
      ServoIzquierda.write(AnguloPosicion);
      ServoDerecha.write(180-AnguloPosicion);
      delay(10);
      AnguloPosicion--;
    }
  }
}