#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define Sensor_entrada_A 14
#define Sensor_saida_A 16
#define Sensor_entrada_B 17
#define Sensor_saida_B 23
#define Led_Red_A 19
#define Led_Green_A 18
#define Led_Blue_A 5
#define Led_Red_B 27
#define Led_Green_B 12
#define Led_Blue_B 4
#define servoPin2 33
#define servoPin1 32
#define buz1 2
#define pot 34

Servo servoMotor1;
Servo servoMotor2;

LiquidCrystal_I2C lcd1(0x27, 20, 4);
LiquidCrystal_I2C lcd2(0x20, 20, 4);

int tempo = 0;
int reset = 0;
int reset_pot = 0;

int contagem1 = 0;
int contagem2 = 0;
int contset1 = 0;
int contset2 = 0;

int servos1 = 0;
int servos2 = 180;

bool tempo_set = 0;
bool direção = 0;
bool S_in_A = 0;
bool S_in_B = 0;
bool S_out_A = 0;
bool S_out_B = 0;
bool S_ias = 0;
bool S_ibs = 0;
bool S_oas = 0;
bool S_obs = 0;




bool setLCD = 0;
bool setLCD2 = 0;


void setup() {

  Serial.begin(115200);

  pinMode(Sensor_entrada_A, INPUT_PULLDOWN);
  pinMode(Sensor_saida_A, INPUT_PULLDOWN);
  pinMode(Sensor_entrada_B, INPUT_PULLDOWN);
  pinMode(Sensor_saida_B, INPUT_PULLDOWN);
  pinMode(Led_Red_A, OUTPUT);
  pinMode(Led_Green_A, OUTPUT);
  pinMode(Led_Blue_A, OUTPUT);
  pinMode(Led_Red_B, OUTPUT);
  pinMode(Led_Green_B, OUTPUT);
  pinMode(Led_Blue_B, OUTPUT);
  pinMode(buz1, OUTPUT);

  servoMotor1.attach(servoPin1);
  servoMotor2.attach(servoPin2);
  
  servoMotor1.write(servos1); 
  servoMotor2.write(servos2); 
  
  lcd1.init();
  lcd1.backlight();
  lcd2.init();
  lcd2.backlight();
  lcd1.setCursor(0,0);
  lcd1.print("");
  lcd1.setCursor(0,1);
  lcd1.print("Display LCD 1");

  lcd2.setCursor(0,0);
  lcd2.print("");
  lcd2.setCursor(0,1);
  lcd2.print("Display LCD 2");
  delay(1500);
  lcd1.clear();
  lcd2.clear();


}
//---------------------------------------------------------------------------
void loop() {

  reset_pot = map(analogRead(pot),0 , 4095, 500, 60000);
  if (contagem1 == 0 && contagem2 == 0){
    if(tempo_set == 1)RGB1(255,255,0);
    if(tempo_set == 1)RGB2(255,255,0);
    if(tempo_set == 0)RGB1(0,0,0);
    if(tempo_set == 0)RGB2(0,0,0);
    servos1 = 0;
    servos2 = 180;
    direcao = 0;
    
  
    
  }
 
  if()

    

  /*
  int tempo = 0;
  int reset = 0;
  int reset_pot = 0;
  int contagem1 = 0;
  int contagem2 = 0;
  int contset1 = 0;
  int contset2 = 0;
  int servos1 = 0;
  int servos2 = 180;
  bool tempo_set = 0;
  bool direção = 0;
  bool S_in_A = 0;
  bool S_in_B = 0;
  bool S_out_A = 0;
  bool S_out_B = 0;
  bool S_ias = 0;
  bool S_ibs = 0;
  bool S_oas = 0;
  bool S_obs = 0;
  bool setLCD = 0;
  bool setLCD2 = 0;
  */


  servoMotor1.write(servos1); 
  servoMotor2.write(servos2); 
  
  delay(10);
  if (tempo == 50)tempo_set = !tempo_set;
  tempo++;
  if(tempo >= 51){
    tempo = 0;
  }
  
}
//---------------------------------------------------------------------------

void RGB1(int red, int Green, int Blue){
  analogWrite(Led_Red_A, red);
  analogWrite(Led_Green_A, Green);
  analogWrite(Led_Blue_A, Blue);
}
void RGB2(int red, int Green, int Blue){
  analogWrite(Led_Red_B, red);
  analogWrite(Led_Green_B, Green);
  analogWrite(Led_Blue_B, Blue);
}