//Robot Ajedrecista con Joystick y pantalla LCD v.2
//02-05-2024
//JEAL
//---------------------------------------------------------------------------------
//Librerias usadas en los Codigos
#include <Servo.h>   //Libreria del Servomotor
#include <LiquidCrystal_I2C.h> //Libreria de la LCD
//-------------------------------------------------------------------------------------
//Especificaciones  de Servos para movimientos
Servo myservoH; //Hombro
Servo myservoC; //Codo
Servo myservoM; //Muñeca
Servo myservoG; //Garra
//Especificaion de LCD con comunicacion I2C
LiquidCrystal_I2C lcd(0x3f,16,2);
//-------------------------------------------------------------------------------
//Variables del Joystick
int Xpin = A3;
int valueX1;
int Ypin = A2;
int valueY1;
int angleX1 = 0;
int angleY1 = 0;
int boton = 2;
//Variables de los Servos
int servoG = 11;
int servoM = 10;
int servoC = 9;
int servoH = 6;
//Variables iniciales de los Servos
int servoposG= 0 ;
int servoposM= 90;
int servoposC= 90;
int servoposH= 90;
//Variables para obtencion de los Pines
int varizquierda = 0;
int varderecha = 0;
int vararriba= 0;
int varabajo = 0;
int vargarra = 0;
boolean flaggarra= false;
//Tiempo de Mov. de los Servos
int tiemposervo = 20;
//--------------------------------------------------------------------------------

//Inicializacion y Declaracion de Salidas y Entradas del Codigo
void setup() {
  myservoH.write(angleX1);
  (myservoC,myservoM).write(angleY1);
  Serial.begin (9600); //Inicializacion de comunicacion serial
//Declaracion de Entradas del Brazo
  pinMode (Xpin, INPUT);
  pinMode (Ypin, INPUT);
  pinMode (boton, INPUT);
  digitalWrite (boton, LOW);
//Declaracion de Salidas del Brazo
  pinMode (servoG, OUTPUT);
  pinMode (servoM, OUTPUT);
  pinMode (servoC, OUTPUT);
  pinMode (servoH, OUTPUT);

  myservoH.attach (servoH);
  myservoC.attach (servoC);
  myservoM.attach (servoM);
  myservoG.attach (servoG);
  delay (2000);  
}
//-----------------------------------------------------------------------------------
void loop() {
//Lectura de Pines Fisicos
  varizquierda = analogRead(Xpin);
  varderecha = analogRead(Xpin);
  vararriba = analogRead(Ypin);
  varabajo = analogRead(Ypin);
  vargarra = digitalRead (boton);
  Serial.print ("Val X= ");
  Serial.print (varizquierda);
  Serial.print ("Val Y= ");
  Serial.print (varderecha);
  Serial.print ("Val Bot= ");
  Serial.println (vargarra);
  delay (100);
  angleX1 = map (valueX1, 0, 1023, 0, 180);
  angleY1 = map (valueY1, 0, 1023, 0, 180);
  myservoH.write(angleX1);
  (myservoC,myservoM).write(angleY1);
//---------------------------------------------------------------------------------
//Movimientos a realizar en el brazo
if (varizquierda > 520) {
  movizquierda();
 } 
if (varderecha < 510) {
  movderecha();
 } 
if (vararriba > 520) {
  movarriba();
 } 
if (varabajo < 510) {
  movabajo();
 }  
if (vargarra == HIGH && flaggarra == false){
  cerrargarra();
  flaggarra= true;
 }
if (vargarra == HIGH && flaggarra == true){
  abrirgarra();
  flaggarra= false;
 }
  delay (50); 
}

void movarriba(){
myservoC.write(servoposC);
myservoM.write(servoposM);
delay (tiemposervo);
servoposC = servoposC + 1;
servoposM = servoposM - 1;
if (servoposC >=120){
  servoposC = 120;
  servoposM = 60;
 }
}

void movabajo(){
  myservoC.write(servoposC);
  delay(tiemposervo);
  servoposC = servoposC - 1;
  if (servoposC <=0){
    servoposC = 0;
  }
}

void movizquierda(){
  myservoH.write(servoposH);
  delay (tiemposervo);
  servoposH = servoposH +1;
  if (servoposH >=180){
    servoposH = 180;
  }
 }

 //Rutina para Movimiento de Hombro a la Derecha
void movderecha(){
  myservoH.write(servoposH);
  delay (tiemposervo);
  servoposH = servoposH - 1;
  if (servoposH <=0){
    servoposH = 0;
  }
 }

 void abrirgarra(){
  while (vargarra == HIGH){
    myservoG.write(servoposG);
    delay (tiemposervo);
    servoposG = servoposG - 1;
    vargarra = digitalRead(boton);
    if (servoposG <= 0){
      servoposG = 0;
    }
  }
}

void cerrargarra(){
  while (vargarra == HIGH){
    myservoG.write(servoposG);
    delay (tiemposervo);
    servoposG = servoposG + 1;
    vargarra = digitalRead(boton);
    if (servoposG >= 180){
      servoposG = 180;
    }
  }
}