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

//*************DECLARACIÓN DE MÓDULOS************

// Paquete A y B 
void GirarMotores(bool, int, int, int, int, int, int);
// Paquete C
void GirarSoporte(int ,bool ,int, int, int);
// Paquete D
void menu_ESTADO();
int ESTADO_SISTEMA(int);
void menu_POSICION();
int VER_POSICION(int);
void menu_VELOCIDAD();
int VELOCIDAD_COMPS(int);
void menu_RESUMEN();
int RESUMEN(int);
void menu();

//Sentido del motor
bool HORARIO = HIGH;
bool ANTIHORARIO = LOW;
volatile bool fin_carrera = true;

//CALIBRAR CON OTRO CÓDIGO POR SEPARADO
const int ANG_A_INI = 30;
const int ANG_A_FIN = 90;

const int ANG_B_INI = 30;
const int ANG_B_FIN = 90;

const int ANG_C_INI = 0;
const int ANG_C_FIN = 120;

//**********MODULO D***************************
Adafruit_FT6206 ctp = Adafruit_FT6206();
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS , TFT_DC,TFT_MOSI,TFT_CLK,TFT_RESET,TFT_MISO); 

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
#define BOXSIZE_2 40
int menu_opcion=0;
int centinela;
TS_Point p;
int ANTERIOR_dist=0;

//************* DEFINICIÓN DE INTERRUPCIÓN -- FINES DE CARRERA A y B************
void ARDUINO_ISR_ATTR ISR_fin_de_carrera(void) {
  fin_carrera = false;
}

//*************DEFINICIÓN DE MÓDULOS************
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 = ctp.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);
    delay(40);
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);

    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 

        }else if(p.y>=2*BOXSIZE+30+2*10 && p.y<=2*BOXSIZE+30+2*10+BOXSIZE){
          Serial.println("Entro en la opcion DETENER");
        }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;
    }
  }
  return 0;
}

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

    TS_Point p = ctp.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);
   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");
      }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");
        }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;
    }
  }
  return 0;
}

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

    TS_Point p = ctp.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);
   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;
    }
  }
  return 0;
}

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

    TS_Point p = ctp.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);
    
    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;
    }
  }
  return 0;
}

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 GirarMotores(bool SENTIDO, int PIN_DIR_A, int PIN_STEP_A, int PIN_EN_A, int PIN_DIR_B, int PIN_STEP_B, int PIN_EN_B){
  Serial.println("Girando ambos motores - paquete A y B");
  // Habilitar ambos motores
  digitalWrite(PIN_EN_A, LOW);
  digitalWrite(PIN_EN_B, LOW);  

  // Establecer direcciones
  digitalWrite(PIN_DIR_A, SENTIDO);
  digitalWrite(PIN_DIR_B, SENTIDO);

  // Generar pulsos para ambos motores
  while (fin_carrera) { 
    digitalWrite(PIN_STEP_A, HIGH);
    digitalWrite(PIN_STEP_B, HIGH);
    delayMicroseconds(1000); 
    digitalWrite(PIN_STEP_A, LOW);
    digitalWrite(PIN_STEP_B, LOW);
    delayMicroseconds(1000);
  }
  Serial.println("Fin de carrera detectado");

  // Deshabilitar ambos motores
  digitalWrite(PIN_EN_A, HIGH);
  digitalWrite(PIN_EN_B, HIGH);

  fin_carrera = true;
  delay(1000); 
}

void GirarSoporte(int FIN_CARRERA, bool SENTIDO, int PIN_DIR, int PIN_STEP, int PIN_EN){
  Serial.println("Girando motor - paquete C");
  if(digitalRead(FIN_CARRERA)==LOW){
    digitalWrite(PIN_EN, HIGH);
    return;
  }
  // Habilitar motor
  digitalWrite(PIN_EN, LOW);
  // Establecer direccion
  digitalWrite(PIN_DIR, SENTIDO);
  while (digitalRead(FIN_CARRERA) == HIGH) { 
    digitalWrite(PIN_STEP, HIGH);
    delayMicroseconds(1000);  
    digitalWrite(PIN_STEP, LOW);
    delayMicroseconds(1000);
  }
  delay(1000); 
}

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); 
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_1, ISR_fin_de_carrera, FALLING);
  attachInterrupt(pulsador2_1, ISR_fin_de_carrera, FALLING);
  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);
  // interrupción para los fines de carrera 
  attachInterrupt(pulsador1_2, ISR_fin_de_carrera, FALLING);
  attachInterrupt(pulsador2_2, ISR_fin_de_carrera, FALLING);
  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); 
  
  myservo_3.attach(SERVO_3, 600, 2400);

  //***************** PAQUETE D *****************
  
  Wire.setPins(10, 8); 

  SPI.begin(15,5,6);
  tft.begin();
  ctp.begin();

  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");

}

void loop() {
  
  GirarSoporte(pulsador1_3, HORARIO, DIR_3, STEP_3, ENABLE_3);  
  Serial.println("Soporte sale");
  myservo_3.write(ANG_C_FIN); 
  Serial.println("se activa servo 3 - sube");
  delay(1000);

  GirarMotores(HORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
  Serial.println("Clamshell sujetado");
  delay(1000);

  GirarSoporte(pulsador2_3, ANTIHORARIO, DIR_3, STEP_3, ENABLE_3); 
  Serial.println("Soporte regresa");
  delay(1000);
  
  myservo_1.write(ANG_A_FIN);  
  Serial.println("se activa servo 1 - baja");
  myservo_2.write(ANG_B_FIN); 
  Serial.println("se activa servo 2 - baja");
  myservo_3.write(ANG_C_INI);
  Serial.println("se activa servo 3 - baja");
  delay(1000);

  Serial.println("Clamshell separado");
  GirarMotores(ANTIHORARIO, DIR_1, STEP_1, ENABLE_1, DIR_2, STEP_2, ENABLE_2);
  myservo_1.write(ANG_A_INI); 
  Serial.println("se activa servo 1 - sube");
  myservo_2.write(ANG_A_INI); 
  Serial.println("se activa servo 2 - sube");
  delay(1000);

  //*********************PAQUETE D************
  menu_opcion=0;
  int x_Pantalla = tft.width();
  int y_Pantalla = tft.height();
  delay(10);
  if (! ctp.touched()) {
    return;
  }

  TS_Point p = ctp.getPoint();

  p.x = map(p.x, 0, 240, 240, 0);
  p.y = map(p.y, 0, 320, 320, 0);
  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();
  }
}