#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Arduino.h>
#include <XPT2046_Touchscreen.h>
#include<ESP32Servo.h>
 //-------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

volatile bool pulsador1PresionadoA = false;
volatile bool pulsador2PresionadoA = false;
volatile bool pulsador1PresionadoB = false;
volatile bool pulsador2PresionadoB = false;

XPT2046_Touchscreen ts(CS_PIN, TIRQ_PIN); 
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS , TFT_DC,TFT_MOSI,TFT_CLK,TFT_RESET,TFT_MISO); 

//-------------MODULO A Y B------------------------

//--------- 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      // Fin de carrera 1
#define pulsador2_1 9      // Fin de carrera 2
ESP32PWM pwm_1;

//----------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      // Fin de carrera 1
#define pulsador2_2 48      // Fin de carrera 2
ESP32PWM pwm_2;

//-----------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      // Fin de carrera 1
#define pulsador2_3 14      // Fin de carrera 2
ESP32PWM pwm_3;

//--------------------------------------------------

// Parámetros de calibración (puedes ajustarlos según el motor)
const int DIST_AGARRE = 1500;    
const int DIST_RETROCESO = 1500; 
const int DIST_INICIO = 2000; 
int clamshell= 0;

//-------------------------------------------------

int centi=1; 
int permitido_1=0; 
unsigned long tiempo;
int touch=1; int touch_2=0; 
//-------------------------------------------
int long anterior_X_S=0; 
int long anterior_Y_S=0; 

#define BOXSIZE 50
#define BOXSIZE_2 30

float ANTERIOR_dist=0;

//#define BOXSIZE_2 40
#define PENRADIUS 3
int oldcolor, currentcolor;
int menu_opcion=0;
int centinela;
TS_Point p;

//Sentido del motor
bool HORARIO = HIGH;
bool ANTIHORARIO = LOW;
int cont_clam=0 ; //contador de los clamshells separados
int mov=0; //centinela del funcionamiento de los paquetes
int angulo=0; 
int validacion_servo; 


void activar_servo(int ini, int fin, Servo myservo){
    for (int i = ini; i <= fin; i++) {
      myservo.write(i);  
      delay(10);  
    }
    for (int i = fin; i >= ini; i--) {
      myservo.write(i); 
      delay(10); 
    }
}

void girar_motor(int DIST, bool SENTIDO, int PIN_DIR, int PIN_STEP, int PIN_EN){
  digitalWrite(PIN_EN, LOW); 
  digitalWrite(PIN_DIR, SENTIDO); 
  for (int i = 0; i < DIST; i++) { 
    digitalWrite(PIN_STEP, HIGH);
    delayMicroseconds(1000); 
    digitalWrite(PIN_STEP, LOW); 
    delayMicroseconds(1000); 
  }
  digitalWrite(PIN_EN, HIGH);
  delay(1000); 
}
//INTERRUPCIONES
void ARDUINO_ISR_ATTR pul1A_ISR(void) { pulsador1PresionadoA = true; }
void ARDUINO_ISR_ATTR pul2A_ISR(void) { pulsador2PresionadoA = true; }

void ARDUINO_ISR_ATTR pul1B_ISR(void) { pulsador1PresionadoB = true; }
void ARDUINO_ISR_ATTR pul2B_ISR(void) { pulsador2PresionadoB = true; }

void setup() {
  Serial.begin(115200);
  SPI.begin(15,5,6);
  tft.begin();
  ts.begin();


  //--------MODULO D--------------------------
  //Se obtiene la resolucion de la pantalla
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  Serial.println(x_Pantalla);
  Serial.println(y_Pantalla);
  
  ts.setRotation(0);
  tft.setRotation(0);
  Serial.println(x_Pantalla);
  Serial.println(y_Pantalla);
  tft.fillRect(0, 0, x_Pantalla, y_Pantalla, ILI9341_BLACK);
 
 //Impresion de los mensajes
  tft.setCursor(26, 120);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.println("Hola :)");

  tft.setCursor(20, 160);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("Seleccione  los modos segun su preferencia.");

  tft.fillScreen(ILI9341_BLACK);
  Serial.println("Termino de pintar");

  //Se establece el menu
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 15);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.println("MENU");

  //OPCIONE VER ESTADO DEL SISTEMA
  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 30+10, BOXSIZE+50, BOXSIZE, ILI9341_GREEN);
  tft.setCursor(90, 45+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("ESTADO DEL ");
  tft.setCursor(98, 55+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("SISTEMA");

  delay(10);
  //OPCION VER LA POSICION DEL CLAMSHELL
  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 10+30+BOXSIZE+10, BOXSIZE+50, BOXSIZE, ILI9341_WHITE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+25, BOXSIZE+50+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("INDICAR");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+22, 50+BOXSIZE+10+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("POSICION");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 60+BOXSIZE+10+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("CLAMSHELL");

  delay(10);
  //OPCION VER VELOCIDAD DE LOS SERVOS Y MOTORES 
  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 2*BOXSIZE+30+2*10+10, BOXSIZE+50, BOXSIZE, ILI9341_BLUE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+13, 2*BOXSIZE+60+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER VELOCIDAD");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+33, 2*BOXSIZE+70+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("SERVOS");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+30, 2*BOXSIZE+80+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("MOTORES");

  //OPCION VER RESUMEN
  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 3*BOXSIZE+60+10, BOXSIZE+50, BOXSIZE, ILI9341_CYAN);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+18, 3*BOXSIZE+60+30);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER RESUMEN");

  Serial.println("");
  //------------------------------------------
  
  //------------MODULO A --------------------
	myservo_1.setPeriodHertz(50);    // 50 hz servo

  pinMode(DIR_1, OUTPUT);  
  pinMode(STEP_1, OUTPUT); 
  pinMode(ENABLE_1, OUTPUT);
  pinMode(pulsador1_1, INPUT_PULLUP); 
  pinMode(pulsador2_1, INPUT_PULLUP); 
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_1, pul1A_ISR, FALLING);
  attachInterrupt(pulsador2_1, pul2A_ISR, FALLING);
  myservo_1.attach(SERVO_1, 600, 2400);
  Serial.println("Motor 1 paso a paso");
  digitalWrite(ENABLE_1, LOW);
  //--------------MODULO B---------------------
	myservo_2.setPeriodHertz(50);    // 50 hz servo

  pinMode(DIR_2, OUTPUT);  
  pinMode(STEP_2, OUTPUT); 
  pinMode(ENABLE_2, OUTPUT);
  pinMode(pulsador1_2, INPUT_PULLUP); 
  pinMode(pulsador2_2, INPUT_PULLUP);
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_2, pul1B_ISR, FALLING);
  attachInterrupt(pulsador2_2, pul2B_ISR, FALLING);
  digitalWrite(ENABLE_2, LOW);

  myservo_2.attach(SERVO_2, 600, 2400);
  Serial.println("Motor 3 paso a paso");

  //--------------MODULO C-----------------
  	myservo_3.setPeriodHertz(50);    // 50 hz servo

  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); 
  
  myservo_3.attach(SERVO_3, 600, 2400);
  Serial.println("Motor 3 paso a paso");

}
void loop(){
  
  menu_opcion=0;
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  delay(10);
  if (! ts.touched()) {
    return;
  }

  TS_Point p = ts.getPoint();
  
 delay(40);

  Serial.println(p.x);
  Serial.println(p.y);

  if(p.x>=(x_Pantalla-(BOXSIZE+50))/2 && p.x<=(x_Pantalla-(BOXSIZE+50))/2+BOXSIZE+50){
    if(p.y>=10+30 && p.y<=10+30+BOXSIZE){
      Serial.println("Ingreso en la opcion ESTADO DEL SISTEMA");
      tft.fillScreen(ILI9341_BLACK);
      Serial.println("Se termmino de pintar el fondo de negro");
      centinela=1;
      delay(10);
      menu_ESTADO();
      while(centinela){
        delay(10);
        if(ESTADO_SISTEMA(centinela)==0){
            menu_opcion=1;          
          break; 
        }
      }
    }else if(p.y>=10+30+BOXSIZE+10 && p.y<=10+30+BOXSIZE+10+BOXSIZE){
      Serial.println("Ingreso en la opcion POSICION DEL CLAMSHELL");
      tft.fillScreen(ILI9341_BLACK);
      Serial.println("Se termmino de pintar el fondo de negro");
      centinela=1;
      delay(10);
      menu_POSICION();
      while(centinela){
        delay(10);
        if(VER_POSICION(centinela)==0){
            menu_opcion=1;          
          break; 
        }
      }
    }else if(p.y>=2*BOXSIZE+30+2*10+10 && p.y<=2*BOXSIZE+30+2*10+10+BOXSIZE){
      Serial.println("Ingreso en la opcion VER VELOCIDAD DE LOS SERVOS Y MOTORES");
      tft.fillScreen(ILI9341_BLACK);
      Serial.println("Se termmino de pintar el fondo de negro");
      centinela=1;
      delay(10);
      menu_VELOCIDAD();
      while(centinela){
        delay(10);
        if(VELOCIDAD_COMPS(centinela)==0){
            menu_opcion=1;          
          break; 
        }
      }
    }else if(p.y>=3*BOXSIZE+60+10 && p.y<=3*BOXSIZE+60+10+BOXSIZE){
      Serial.println("Ingreso a la opcion VER RESUMEN");
      tft.fillScreen(ILI9341_BLACK);
      Serial.println("Se termmino de pintar el fondo de negro");
      centinela=1;
      delay(10);
      menu_RESUMEN();
      while(centinela){
        if(RESUMEN(centinela)==0){
            delay(10);
            menu_opcion=1;          
          break; 
        }
      }
    }
  }
  if(menu_opcion==1){
    tft.fillScreen(ILI9341_BLACK);
    menu();
  }
    
}

void menu(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();

  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 15);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);
  tft.println("MENU");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 30+10, BOXSIZE+50, BOXSIZE, ILI9341_GREEN);
  tft.setCursor(90, 45+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("ESTADO DEL ");
  tft.setCursor(98, 55+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("SISTEMA");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 10+30+BOXSIZE+10, BOXSIZE+50, BOXSIZE, ILI9341_WHITE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+25, BOXSIZE+50+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("INDICAR");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+22, 50+BOXSIZE+10+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("POSICION");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+20, 60+BOXSIZE+10+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("CLAMSHELL");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 2*BOXSIZE+30+2*10+10, BOXSIZE+50, BOXSIZE, ILI9341_BLUE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+13, 2*BOXSIZE+60+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER VELOCIDAD");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+33, 2*BOXSIZE+70+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("SERVOS");
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+30, 2*BOXSIZE+80+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("MOTORES");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 3*BOXSIZE+60+10, BOXSIZE+50, BOXSIZE, ILI9341_CYAN);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+18, 3*BOXSIZE+60+30);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("VER RESUMEN");
}

int ESTADO_SISTEMA(int centinela){ 
  while(centinela){
    int x_Pantalla = tft.width();
    int y_Pantalla = tft.height();
    delay(10);

    TS_Point p = ts.getPoint();
    
   delay(40);


    Serial.println(p.x);
    Serial.println(p.y);

    if(anterior_X_S!=p.x || anterior_Y_S!=p.y){
      Serial.println("ENTRO NUEVO PUNTO");
      if(p.x>=(x_Pantalla-(BOXSIZE+50))/2 && p.x<=(x_Pantalla-(BOXSIZE+50))/2+BOXSIZE+50){
        if((p.y>=40+BOXSIZE) && p.y<=40+2*BOXSIZE){
          Serial.println("Entro en la opcion INICIAR");
          //Se inicia el funcionamiento 
          mov=1;
          while(mov==1);
          modulo_A();
          //modulo_C();
          modulo_B();
        }else if(p.y>=2*BOXSIZE+30+2*10 && p.y<=2*BOXSIZE+30+2*10+BOXSIZE){
          Serial.println("Entro en la opcion DETENER");
          mov=0; 
        }else if(p.y>=3*BOXSIZE+60 && p.y<=3*BOXSIZE+60+BOXSIZE){
          Serial.println("Entro en la opcion ATRAS");
          return centinela=0; 
        }
      }
      anterior_X_S=p.x;
      anterior_Y_S=p.y;
    }
  }
}

int VER_POSICION(int centinela){
 while(centinela){
    int x_Pantalla = tft.width();
    int y_Pantalla = tft.height();
    delay(10);

    TS_Point p = ts.getPoint();
    
   delay(40);


    Serial.println(p.x);
    Serial.println(p.y);

  //-----SENSOR INFRARROJO------------------------
  
    float sensor=analogRead(12);
    Serial.println(sensor);
    float dist_cm=map(sensor,0,4095,0,20);
    float dist_mm;
    dist_mm=dist_cm*10; 
    dist_mm=round(dist_mm/5)*5;
    Serial.print("sensor: ");
    Serial.println(dist_mm);
    if(dist_mm!=ANTERIOR_dist){
      tft.fillRect(10, 50, 100, 100, ILI9341_BLACK);
      tft.setCursor(20,50);
      tft.setTextColor(ILI9341_WHITE);
      tft.setTextSize(1);
      tft.println(dist_mm);

      if(dist_cm>=2 && dist_cm<=15){
        Serial.println("Se detecta clamshell");
        cont_clam++; 
      }else{
        Serial.println("No se detecta clamshell");
      }

      ANTERIOR_dist=dist_mm;
      
    }

    if(anterior_X_S!=p.x || anterior_Y_S!=p.y){
      Serial.println("ENTRO NUEVO PUNTO");
      
     if(p.y>=y_Pantalla-(BOXSIZE_2) && p.y<=y_Pantalla-1){
        if((p.x>=0) && p.x<BOXSIZE_2){
          Serial.println("Entro en la opcion STOP");
          mov=0;
        }else if(p.x>x_Pantalla-BOXSIZE_2 && p.x<=x_Pantalla-1){
          Serial.println("Entro en la opcion MENU");
          return centinela=0; 
        }
      }
      anterior_X_S=p.x;
      anterior_Y_S=p.y;
    }
  }
}
int VELOCIDAD_COMPS(int centinela){
 while(centinela){
    int x_Pantalla = tft.width();
    int y_Pantalla = tft.height();
    delay(10);
    //if (! ctp.touched()) {
      //return;
    //}

    TS_Point p = ts.getPoint();
    
   delay(40);

    Serial.println(p.x);
    Serial.println(p.y);

    if(anterior_X_S!=p.x || anterior_Y_S!=p.y){
      Serial.println("ENTRO NUEVO PUNTO");
      
      if(p.y>=y_Pantalla-(BOXSIZE_2) && p.y<=y_Pantalla-1){
        if((p.x>=0) && p.x<BOXSIZE_2){
          Serial.println("Entro en la opcion STOP");
        }else if(p.x>x_Pantalla-BOXSIZE_2 && p.x<=x_Pantalla-1){
          Serial.println("Entro en la opcion MENU");
          return centinela=0; 
        }
      }
      anterior_X_S=p.x;
      anterior_Y_S=p.y;
    }
  }
}

int RESUMEN(int centinela){
  while(centinela){
    int x_Pantalla = tft.width();
    int y_Pantalla = tft.height();
    delay(10);

    TS_Point p = ts.getPoint();
    
    delay(40);

    Serial.println(p.x);
    Serial.println(p.y);

    if(anterior_X_S!=p.x || anterior_Y_S!=p.y){
      Serial.println("ENTRO NUEVO PUNTO");
          
          Serial.println("Entro en la opcion MENU");
          Serial.print("Se han procesado ");
          Serial.print(cont_clam);
          Serial.println("clamshells");
          return centinela=0; 
      if(p.y>=330 && p.y<=3600){
        if((p.x>=500) && p.x<700){
          Serial.println("Entro en la opcion STOP");
          mov=0; 
        }else if(p.x>700 && p.x<800){
          Serial.println("Entro en la opcion MENU");
          return centinela=0; 
        }
      }
      anterior_X_S=p.x;
      anterior_Y_S=p.y;
    }
  }
}

void menu_VELOCIDAD(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  tft.fillRect(0, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(10,y_Pantalla-BOXSIZE_2+15);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("STOP");

  tft.fillRect(x_Pantalla-BOXSIZE_2, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(x_Pantalla-BOXSIZE_2+10,y_Pantalla-BOXSIZE_2+15);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("MENU");
}

void menu_RESUMEN(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  tft.fillRect(0, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(10,y_Pantalla-BOXSIZE_2+15);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("STOP");

  tft.fillRect(x_Pantalla-BOXSIZE_2, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
  tft.setCursor(x_Pantalla-BOXSIZE_2+10,y_Pantalla-BOXSIZE_2+15);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);
  tft.println("MENU");
}

void menu_ESTADO(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 10+30+BOXSIZE, BOXSIZE+50, BOXSIZE, ILI9341_WHITE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+25, BOXSIZE+60);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("INICIAR");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 2*BOXSIZE+30+2*10, BOXSIZE+50, BOXSIZE, ILI9341_BLUE);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+25, 2*BOXSIZE+70);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("DETENER");

  tft.fillRect((x_Pantalla-(BOXSIZE+50))/2, 3*BOXSIZE+60, BOXSIZE+50, BOXSIZE, ILI9341_CYAN);
  tft.setCursor((x_Pantalla-(BOXSIZE+50))/2+30, 3*BOXSIZE+70+10);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(1);
  tft.println("ATRAS");
}
void menu_POSICION(){
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
    
    tft.fillRect(0, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
    tft.setCursor(10,y_Pantalla-BOXSIZE_2+15);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("STOP");

    tft.fillRect(x_Pantalla-BOXSIZE_2, y_Pantalla-BOXSIZE_2, BOXSIZE_2, BOXSIZE_2, ILI9341_RED);
    tft.setCursor(x_Pantalla-BOXSIZE_2+10,y_Pantalla-BOXSIZE_2+15);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(1);
    tft.println("MENU");
    
}


void modulo_A(){
  if (pulsador1PresionadoA) {
    girar_motor(DIST_INICIO, ANTIHORARIO,  DIR_1,  STEP_1, ENABLE_1);
    pulsador1PresionadoA = false;
    Serial.println("fin de carrera 1 retrocediendo");
  }

  if (pulsador2PresionadoA) {
    girar_motor(DIST_INICIO,  HORARIO,  DIR_1,  STEP_1, ENABLE_1);
    pulsador2PresionadoA = false; 
    Serial.println("fin de carrera 2  avanzando");
  }
  girar_motor(DIST_AGARRE,  HORARIO,  DIR_1,  STEP_1, ENABLE_1);
  myservo_1.write(90); 
  delay(500);
  girar_motor(DIST_RETROCESO,  ANTIHORARIO,  DIR_1,  STEP_1, ENABLE_1);
  myservo_1.write(30); 
  delay(500);
}



void modulo_B(){
  if (pulsador1PresionadoB) {
    girar_motor(DIST_INICIO, ANTIHORARIO,  DIR_2,  STEP_2, ENABLE_2);
    pulsador1PresionadoB = false;
    Serial.println("fin de carrera 1 retrocediendo");
  }

  if (pulsador2PresionadoB) {
    girar_motor(DIST_INICIO,  HORARIO,  DIR_2,  STEP_2, ENABLE_2);
    pulsador2PresionadoB = false; 
    Serial.println("fin de carrera 2  avanzando");
  }
  girar_motor(DIST_AGARRE,  HORARIO,  DIR_2,  STEP_2, ENABLE_2);
  myservo_2.write(90); 
  delay(500);
  girar_motor(DIST_RETROCESO,  ANTIHORARIO,  DIR_2,  STEP_2, ENABLE_2);
  myservo_2.write(30); 
  delay(500);
}

/*
void modulo_C(){
  bool centinela = true;
  bool centinela_2 = true;
  bool validacion = false;
  //int limit_2 = digitalRead(pin_limit2);
  if(digitalRead(pulsador1_3)!=0){
    digitalWrite(DIR_3, HIGH);
    while(centinela){
      digitalWrite(STEP_3,HIGH);
      delay(100);
      digitalWrite(STEP_3,LOW);
      delay(100);
      if(digitalRead(pulsador1_3)== 0){
        centinela = false;
        validacion= true;
        //una vez hecho esto ya se encuentra calibrada la posicion del motor
      Serial.println("SE CALIBRO CON EXITO");
      delay(50);
      }
    }
  }
  if (digitalRead(pulsador1_3)== 0 ){
    validacion=true;
    //ya esta en la posicion de calibracion
    Serial.println("SE CALIBRO CON EXITO 2");
    delay(50);
  }  
  delay(50);
  //UNA VEZ YA CALIBRADO, SE DIRIGE HACIA OTRO LADO
  if((digitalRead(pulsador2_3)!=0) && (validacion)){
    Serial.write("hola");
    digitalWrite(DIR_3, LOW);
    while(centinela_2){
      digitalWrite(STEP_3,HIGH);
      delay(100);
      digitalWrite(STEP_3,LOW);
      delay(100);
      if(digitalRead(pulsador2_3)== 0){
        centinela_2 = false;
        validacion_servo = 1;
        //se llego al otro limite 
      Serial.println("SE LLEGO AL SIGUIENTE LIMITE / LIMITE 2");
      delay(50);
      }
    }
  } 

  if (digitalRead((pulsador2_3)==0)&& validacion){
    Serial.println("SE llego al siguiente limite");
    validacion_servo = 1;
    delay(50);
  } 
  //AQUI SE UTILIZA EL SERVOMOTOR
  if(validacion_servo){
    funcionamiento_servo();
    delay(10);
  }
}
void funcionamiento_servo(){
    for(angulo; angulo<120;angulo++){
      if(angulo>=120){
        angulo=120;
      }
      myservo_3.write(angulo);
      delay(50);
    }
    for(angulo; angulo>0;angulo--){
      if(angulo<=0){
        angulo=0;
      }
      myservo_3.write(angulo);
      delay(50);
    }
}
*/