//<<<<Libreria para control de servomotores>>>>
#include <Servo.h>

int ejeX = A0;
int ejeY = A1;
int SW = A2;

int lecturaX = 0;
int lecturaY = 0;
int lecturaSW = 0;

int grabaX = 11;
int lecturaGrabaX = 0;

//>>> Base 10° - 170°
//>>> Hombro 180° - 150
//>>> Escalera 110 - 160
//>>> Griper

//<<<<Guarda datos>>>>
int posiciones [150]; //>>>>Posiciones de la Base
int posicionesE [150]; //>>>>Posiciones de la Escalera
int posicionesH [150]; //>>>>Posiciones Hombro
int posicionesG [150]; //>>>>Posiciones Griper

int angulo = 0;
int anguloE = 0;
int anguloH = 0;
int anguloG = 0;


//<<<<Pushboton>>>>
int conta = 0;

//<<<<Donde se almacenan los 150 datos>>>>
int datos = 0;

Servo myServo1; //>>>>Servo 1 GRIPER
Servo myServo2; //>>>>Servo 2 BASE
Servo myServo3; //>>>>Servo 3 ESCALERA
Servo myServo4; //>>>>Servo 4 HOMBRO

int x1 = 90; //>>>>Posicion
int y1 = 180; //>>>>Hombro
int y2 = 110; //>>>>Escalera
int DATOS = 0; //>>>>Datos recibidos

int a = 0;
int b = 0;
int c = 0;
int d = 0;

int estadoGrip = 50; //>>>>Lectura de Griper
int contador = 0;

void setup()
{
  Serial.begin(9600), //>>>>Comunicacion a 9600
  myServo1.attach(5); //>>>>Griper
  myServo2.attach(2); //>>>>Base
  myServo3.attach(4); //>>>>Escalera
  myServo4.attach(3); //>>>>Hombro

  myServo1.write(50); //>>>>Griper
  myServo2.write(90); //>>>>Base
  myServo3.write(105); //>>>>Escalera
  myServo4.write(175); //>>>>Hombro

  pinMode(grabaX, INPUT);

  pinMode(ejeX, INPUT);
  pinMode(ejeY, INPUT);
  pinMode(SW, INPUT); //>>>>Señal de boton como entrada digital

  delay(1000);

}

void loop()
{
  lecturaX = analogRead(ejeX);
  lecturaY = analogRead(ejeY);
  lecturaSW = analogRead(SW);
  lecturaGrabaX = analogRead(grabaX);
  delay(30);

  Serial.print( "  X:  ");
  Serial.print(lecturaX);
  Serial.print("  Y:  ");
  Serial.println(lecturaY);

  //<<<<Botones para iniciar grabacion de grados>>>>

  if(lecturaGrabaX == 1)
  {
    conta++;
    delay(1);
    lecturaGrabaX = 0;
    delay(100);
    if(conta == 3)
    {
      Serial.println("Stop");
      
      conta = 0;
      delay(1000);
    }
  }

  Serial.print("CONTA UNO: ");
  Serial.print(conta);
  Serial.print(", ");
  //<<<<Si no se han presionado ningun boton, actual con normalidad>>>>

  if(conta == 0)
  {
    Serial.print("\t SIN GRABAR");

    //<<<<Griper>>>>
    if(lecturaSW <= 500)
    {
      contador++;
      delay(1);
      lecturaSW = 1000;
      delay(100);
    }

    if(contador == 1)
    {
      estadoGrip = 160;
      myServo1.write(160); //>>>>Griper
    }

    if(contador == 2)
    {
      estadoGrip = 30;
      myServo1.write(30);
      contador = 0;
    }
    //<<<<Base (Izquierda - Derecha) >>>>
    if(lecturaX <= 200) //Si tenemos un "4" grados derecha
    {
      if(c == 0) //Si la bandera c=0 aunmentamos la posicion
      {
        x1 = x1 + 2; //x= datos que aumentan para mover la base
      }

      if(x1 == 170) //Si ya llego al limite = 170 se detenga
      {
        x1 =170;
        myServo2.write(170);
        c = 1; //Bandera c=1 para que ya no aumente posiciones
      }
      else
      {
        myServo2.write(x1); //Se mueve la base
        delay(5);
        myServo1.write(estadoGrip); //Escribimos la posicion del griper
        d = 0; //Bandera d=0 para que pueda Retornar la base
      }
    }

    x1 = x1;

    //<<<<Retorno>>>>
    if(lecturaX >= 800) //Si envia "3" comenzamos a retornar
    {
      if(d == 0) //d=0 Retornar en cualquier momento
      {
        x1 = x1 - 2; //x1-1 Retorno de posicion
      }
      if(x1 == 10) //Limite x=10 grados
      {
        x1 = 10;
        myServo2.write(10); //Se queda en 10 grados
        d = 1;             //d=1 No rebasa el limite
      }
      else
      {
        myServo2.write(x1); //Base retorna
        delay(5);
        myServo1.write(estadoGrip); //Griper puede cambiar cuando sea
        c = 0;
      }
    }

    //<<<<Grua (Subir - Bajar)>>>>
    if(lecturaY >= 800)
    {
      myServo1.write(estadoGrip); //Griper puede cambiar cuando sea
      if(a == 0) //Bandera a=0 se puede desender
      {
        y1 = y1 - 1; //Hombro inicia en 180 y termina en 130
        y2 = y2 + 1; //Escalera inicia en 70 - Escalera baja hasta 120
      }

      if(y1 == 160 && y2 == 110) //Limites, si ya cumplio los limites de posicion se mantienen en el ultimo estado
      {
        y1 = 110;
        y2 = 160;
        myServo3.write(110);
        myServo4.write(160);
        a = 1;
      }
      else
      {
        myServo3.write(y2);
        myServo4.write(y1);
        delay(5);
        b = 0;
      }
    }

    if(lecturaY <= 200)
    {
      myServo1.write(estadoGrip);
      if(b == 0)
      {
        y1 = y1 + 1;
        y2 = y2 -1;
      }

      if(y1 == 180 && y2 == 160)
      {
        y1 = 180;
        y2 = 160;
        myServo3.write(160);
        myServo4.write(180);
        b = 1;
      }
      else
      {
        myServo3.write(y2);
        myServo4.write(y1);
        delay(5);
        a = 0;
      }
    }

    if(lecturaX >= 201 && lecturaX <= 699 && lecturaY >= 201 && lecturaY <= 699)
    //Mantiene la posicion donde la dejamos
    {
      myServo1.write(estadoGrip);
      myServo2.write(x1);
      myServo3.write(y2);
      myServo4.write(y1);
      delay(1);
    }
  }

  //<<<<Grabando posiciones en X, Y, Griper>>>>
  if(conta == 1)
  {
    if(lecturaSW <= 500)
    {
      contador++;
      delay(1);
      lecturaSW = 1000;
      delay(100);
    }

    if(contador == 1)
    {
      estadoGrip = 160;
      myServo1.write(160);
    }
    if(contador == 2)
    {
      estadoGrip = 30;
      myServo1.write(30);
      contador = 0;
    }

    Serial.print(" ** GRABANDO EN Y **");

    //<<<<Grua (Subir y bajar) >>>>
    if(lecturaY >= 800)
    {
      myServo1.write(estadoGrip);
      if(a == 0)
      {
        y1 = y1 - 1;
        y2 = y2 + 1;
      }
      
      if(y1 == 160 && y2 == 110)
      {
        y1 = 110;
        y2 = 160;
        myServo3.write(110);
        myServo4.write(160);
        a = 1;
      }
      else
      {
        myServo3.write(y2);
        myServo4.write(y1);
        delay(5);
        b = 0;
      }
    }

    if(lecturaY <= 200)
    {
      myServo1.write(estadoGrip);
      if(b == 0)
      {
        y1 = y1 + 1;
        y2 = y2 - 1;
      }

      if(y1 == 180 && y2 ==160)
      {
        y1 = 180;
        y2 = 160;
        myServo3.write(160);
        myServo4.write(180);
        b = 1;
      }
      else
      {
        myServo3.write(y2);
        myServo4.write(y1);
        delay(5);
        a = 0;
      }
    }
    Serial.print(" ** GRABANDO EN X **");

    if(lecturaX >= 800)
    {
      if(c == 0);
      {
        x1 = x1 + 2;
      }
      if(x1 == 170)
      {
        x1 = 170;
        myServo2.write(170);
        c = 1;
      }
      else
      {
        myServo2.write(x1);
        delay(10);
        myServo1.write(estadoGrip);
        d = 0;
      }
    }
    x1 = x1;

    //<<<<Retorno>>>>
    if(lecturaX <= 200)
    {
      if(d == 0)
      {
        x1 = x1 - 2; 
      }
      if(x1 == 10)
      {
        x1 = 10;
        myServo2.write(10);
        d = 1;
      }
      else
      {
        myServo2.write(x1);
        delay(5);
        myServo1.write(estadoGrip);
        c = 0;
      }
    }
    if(lecturaX >= 201 && lecturaX <= 699 && lecturaY >= 201 && lecturaY <= 699)
    {
      myServo1.write(estadoGrip);
      myServo2.write(x1);
      myServo3.write(y2);
      myServo4.write(y1);
      delay(10);
    }

    angulo = x1;
    conta = 1;
    anguloE = y2;
    anguloH = y1;
    anguloG = estadoGrip;

    if(datos < 150)
    {
      posiciones[datos] = angulo;
      posicionesE[datos] = anguloE;
      posicionesH[datos] = anguloH;
      posicionesG[datos] = anguloG;
      Serial.print("A_B: ");
      Serial.print(angulo);
      Serial.print("A_E: ");
      Serial.print(anguloE);
      Serial.print("A_H: ");
      Serial.print(anguloH);
      Serial.print("A_G: ");
      Serial.print(anguloG);
      myServo2.write(angulo);
      myServo3.write(posicionesE[datos]);
      myServo4.write(posicionesH[datos]);
      datos++;
      delay(30);
    }
  }
  //<<<<Reproducir posiciones>>>>
  if(conta ==2)
  {
    int i = 0;
    for(i = 0; i < 150; i + 1)
    {
      i = i + 1;
      Serial.print("BASE");
      Serial.print(posiciones[i]);
      Serial.print("ESCALERA");
      Serial.print(posicionesE[i]);
      Serial.print("HOMBRO");
      Serial.print(posicionesH[i]);
      Serial.print("GRIPER");
      Serial.print(posicionesG[i]);

      myServo1.write(posicionesG[i]);
      myServo2.write(posiciones[i]);
      myServo3.write(posicionesE[i]);
      myServo4.write(posicionesH[i]);
      delay(50);
      Serial.print("Reproduciendo Posiciones");
    }

    if(conta == 3)
    {
      Serial.print("Stop");

      conta = 0;
      delay(5000);
    }
  }
}