// Sistemas Embebidos
// Ejercicio Grúa Inf6
// Reina Edwin - Jami Byron Gr1-3

#include <Servo.h>
const int ledPin = 13;  // Declaración pin de los servos 10 y 11
const int x_Pin = A0;   // eje x y eje y del joystic        
const int y_Pin = A1;
const int Boton_Pin = 2; // Boton del joystic
const int servoxPin = 10;
const int servoyPin = 11; 

int x_Valor;     // Valor para eje x
int y_Valor; // Valor para el eje y
int X_pos = 90;   // Posición inicial del servo 1 y 2
int Y_pos = 90;

volatile int switchState = 0; // variable  decambio
Servo servox;  // Declaración de servos
Servo servoy;

void setup() 
{
  pinMode (x_Pin, INPUT) ;              // Entradas servos       
  pinMode (y_Pin, INPUT) ;
  pinMode(ledPin, OUTPUT);
  pinMode(Boton_Pin, INPUT_PULLUP);  // Boton joystic
  servox.attach(servoxPin );   // Lectura del servo
  servoy.attach(servoyPin); 
  servox.write(X_pos);        // Impresion de posición
  servoy.write(Y_pos);
  attachInterrupt(digitalPinToInterrupt(Boton_Pin), switchISR, CHANGE); // Interrupcion de boton
}

void loop() 
{
  x_Valor = analogRead(x_Pin);        // Lectura de los servos
  y_Valor = analogRead(y_Pin);    // Y guardo en la variable
// For X-Axis Servo
  if (x_Valor < 300)    // condicon valor analogico de la posición servo para eje x
  {
    if (X_pos < 10) 
    {
                      // No realiza nada
    } 
    else
    {
      X_pos = X_pos - 5;      // Caso contrario reduce andulo de servo e imprime
      servox.write(X_pos);
      delay(100);         // Tiempo 0.1 seg
    } 
  } 
  
  if (x_Valor > 700) // Condición valor analogico de lal posición servo para eje x
  {
    if (X_pos > 160)
    {
              // No realizo nada
    }
    else
    {
      X_pos = X_pos + 5;  // Caso contrario incremento angulo e imprime
      servox.write(X_pos);
      delay(100);
    }
  }
// For Y-Axis Servo
if (y_Valor < 300)     // condicon valor analogico de la posición servo para eje y
  {
    if (Y_pos < 10) 
    {
      // Do Nothing 
    } 
    else
    {
      Y_pos = Y_pos - 5;        // Caso contrario decremento angulo e imprime
      servoy.write(Y_pos);
      delay(100); 
    } 
  } 
  
  if (y_Valor > 700)         // condicon valor analogico de la posición servo para eje y
  {
    if (Y_pos > 160)
    {
      // Do Nothing  
    }
    else            // Caso contrario incremento angulo e imprime
    {
      Y_pos = Y_pos + 5;
      servoy.write(Y_pos);
      delay(100);
    }
  }
}

void switchISR()
{
  switchState = digitalRead(Boton_Pin);     // Leo pin boton y establezco en estado switch
  digitalWrite(ledPin, switchState);
}