#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Arduino.h>
//#include <XPT2046_Touchscreen.h>
#include<ESP32Servo.h>
 
//***************** PAQUETE A *****************
Servo myservo_1;
#define SERVO_1 17
#define ENABLE_1 18
#define DIR_1 8           
#define STEP_1 3          
#define pulsador1_1 10     
#define pulsador2_1 9      

//***************** PAQUETE B *****************
Servo myservo_2;
#define SERVO_2 36
#define ENABLE_2 37
#define DIR_2 38           
#define STEP_2 39          
#define pulsador1_2 35      
#define pulsador2_2 48      

//***************** PAQUETE C *****************
Servo myservo_3;
#define SERVO_3 13
#define ENABLE_3 20
#define DIR_3 21          
#define STEP_3 47          
#define pulsador1_3 19      
#define pulsador2_3 14      

//***************** PAQUETE D *****************
#define SENSOR 12

#define TFT_DC 42
#define TFT_CS 1
#define TFT_MOSI 41
#define TFT_CLK 40
#define TFT_MISO 16
#define TFT_RESET 2

#define TIRQ_PIN 4
#define TS_DO 5
#define TS_DIN 6
#define TS_CLK 15
#define CS_PIN 7

//Sentido del motor
bool HORARIO = HIGH;
bool ANTIHORARIO = LOW;

//Variables para contador de clamshell
int Contador = 0;  
int Estado = 0;
int EstadoAnterior = 1; 

const int ANG_A_INI = 100;
const int ANG_A_FIN = 150;

const int ANG_B_INI = 100;
const int ANG_B_FIN = 50;

const int ANG_C_INI = 80;
const int ANG_C_FIN = 0;

void CalibrarActuadores(){

  myservo_1.write(ANG_A_INI);
  myservo_2.write(ANG_B_INI);
  myservo_3.write(ANG_C_INI);
  // Girar ambos motores hasta que ambos alcancen su limit switch
  // Habilitar ambos motores
  digitalWrite(ENABLE_1, LOW);
  digitalWrite(ENABLE_2, LOW);  
  digitalWrite(ENABLE_3, LOW);

  // Establecer direcciones
  digitalWrite(DIR_1, ANTIHORARIO);
  digitalWrite(DIR_2, ANTIHORARIO);
  digitalWrite(DIR_3, ANTIHORARIO);

  do{
    digitalWrite(STEP_1, HIGH);
    delay(5); 
    digitalWrite(STEP_1, LOW);
    delay(5); 
  }while(digitalRead(pulsador2_1) == HIGH);

  do{
    digitalWrite(STEP_2, HIGH);
    delay(5); 
    digitalWrite(STEP_2, LOW);
  }while(digitalRead(pulsador2_2) == HIGH);

  do{
    digitalWrite(STEP_3, HIGH);
    delay(5);     
    digitalWrite(STEP_3, LOW);
    delay(5);
  }while(digitalRead(pulsador2_3) == HIGH);
  
  // desaabilitar ambos motores
  digitalWrite(ENABLE_1, HIGH);
  digitalWrite(ENABLE_2, HIGH);  
  digitalWrite(ENABLE_3, HIGH);

}


void setup() {
  Serial.begin(115200);
  //***************** PAQUETE A *****************
  pinMode(DIR_1, OUTPUT);  
  pinMode(STEP_1, OUTPUT); 
  pinMode(ENABLE_1, OUTPUT);
  pinMode(pulsador1_1, INPUT_PULLUP); 
  pinMode(pulsador2_1, INPUT_PULLUP); 
  digitalWrite(ENABLE_1, LOW);
  myservo_1.attach(SERVO_1, 600, 2400);

  //***************** PAQUETE B *****************
  pinMode(DIR_2, OUTPUT);  
  pinMode(STEP_2, OUTPUT); 
  pinMode(ENABLE_2, OUTPUT);
  pinMode(pulsador1_2, INPUT_PULLUP); 
  pinMode(pulsador2_2, INPUT_PULLUP);
  digitalWrite(ENABLE_2, LOW);
  myservo_2.attach(SERVO_2, 600, 2400);

  //***************** PAQUETE C *****************
  pinMode(DIR_3, OUTPUT);  
  pinMode(STEP_3, OUTPUT); 
  pinMode(ENABLE_3, OUTPUT); 
  pinMode(pulsador1_3, INPUT_PULLUP); 
  pinMode(pulsador2_3, INPUT_PULLUP);
  digitalWrite(ENABLE_3, LOW); 
  
  pinMode(SENSOR, INPUT); 

  myservo_3.attach(SERVO_3, 600, 2400);

  CalibrarActuadores();
}


void loop() {
}