#include <ESP32Servo.h>

Servo primeiroServo;
Servo segundoServo;

int pinoLed1 = 32;
int pinoLed2 = 33;
int pinoLedOK = 21;

int pinoServo1 = 26;
int pinoServo2 = 25;

int pinoLeituraServo1 = 27;
int pinoLeituraServo2 = 12;

bool statusServo1 = true;
bool statusServo2 = true;

int posicaoServo1 = 0;
int posicaoServo2 = 0;

int tempoTrabalhado = 0;
int tempoTrabalhadoServo1 = 0;
int tempoTrabalhadoServo2 = 0;

int orientacao = 0;


void setup() {
  // put your setup code here, to run once:
  pinMode(pinoLeituraServo1, INPUT);
  pinMode(pinoLeituraServo2, INPUT);
  

  pinMode(pinoLed1, OUTPUT);
  pinMode(pinoLed2, OUTPUT);
  pinMode(pinoLedOK, OUTPUT);
  
  Serial.begin(115200);

  digitalWrite(pinoLed1, LOW);
  digitalWrite(pinoLed2, LOW);

  primeiroServo.attach(pinoServo1);
  segundoServo.attach(pinoServo2);

  primeiroServo.write(0);
  segundoServo.write(180);
}

void loop() {

  
    leituraMotores();

   
   if(millis() - tempoTrabalhado > 100){

      
      moverServos();

      
     tempoTrabalhado = millis();

    }
    
    


}


void moverServos(){
      if(orientacao > 180){
        orientacao = 0;
      }

      int orientacaoReversa = map(orientacao, 0, 180, 180, 0);
      primeiroServo.write(orientacao);
      segundoServo.write(orientacaoReversa);
      orientacao++;
}

void leituraMotores(){
  int leituraServo1 = digitalRead(pinoLeituraServo1);
  int leituraServo2 = digitalRead(pinoLeituraServo2);

  if(leituraServo1 != 0){tempoTrabalhadoServo1 = millis();}
  if(leituraServo2 != 0){tempoTrabalhadoServo2 = millis();}

  if(leituraServo1 == 0){

    if(statusServo1 && statusServo2){
      digitalWrite(pinoLedOK, HIGH);
    }else{
      digitalWrite(pinoLedOK, LOW);
    }

    if(millis() - tempoTrabalhadoServo1 > 150){
      statusServo1 = false;
      digitalWrite(pinoLed1, HIGH);

    
    }else{
      digitalWrite(pinoLed1, LOW);
      statusServo1 = true;
    }
  }

  if (leituraServo2 == 0){
    if(millis() - tempoTrabalhadoServo2 > 150){
      statusServo2 = false;
      digitalWrite(pinoLed2, HIGH);
 
    }else{
      statusServo2 = true;
      digitalWrite(pinoLed2, LOW);
    }
    
  }

    
}