#include <Servo.h>

//puertos para los joystick
#define joystick_x1 A0
#define joystick_y1 A1
#define joystick_x2 A2
#define joystick_y2 A3
#define pot A4

// nombre de los servos
Servo garra;
Servo base;
Servo brazo;
Servo antebrazo;
Servo muneca;
Servo giromuneca;

//variables para los angilos y valor de los joystick
int angulo1 = 90; //servo base
int angulo2 = 90; //servo antebrazo
int angulo3= 90;  //servo brazo
int angulo4 = 90; // servo muñeca
int angulo5 = 90; // serbo giro muñeca
int angulo6= 90;  // servo garra
int valor_joystick_x1;
int valor_joystick_y1;
int valor_joystick_x2;
int valor_joystick_y2;
int valor_pot;
int valor_B1 = 0;
int valor_B2 = 0;


void setup() {
  // se declaran los pines de los servos y de los botones de los joystick
  pinMode(4, INPUT_PULLUP); // INPUT_PULLUP para usar la resistencia interna de la placa arduino
  pinMode(7, INPUT_PULLUP);
  Serial.begin(9600);
  garra.attach(11);
  giromuneca.attach(3);
  muneca.attach(5);
  antebrazo.attach(9);
  brazo.attach(10);
  base.attach(6);
  // pongo servos a 90 grados
  garra.write(angulo6);
  base.write(angulo1);
  brazo.write(angulo3);
  antebrazo.write(angulo2);
  muneca.write(angulo4);
  giromuneca.write(angulo5);
  
}

void loop() {
  // leyendo valores de los joystick
  valor_joystick_x1 = analogRead(joystick_x1);
  valor_joystick_y1 =  analogRead(joystick_y1);
  valor_joystick_x2 = analogRead(joystick_x2);
  valor_joystick_y2 =  analogRead(joystick_y2);
  valor_pot =  analogRead(pot);
  valor_B1 = digitalRead(4);
  valor_B2 = digitalRead(7);

  //comparaciones control 1
  if(valor_joystick_x1 < 340) angulo2 -=10;
  else if(valor_joystick_x1 > 680) angulo2 +=10;

  if(valor_joystick_y1 < 340) angulo3 -=10;
  else if(valor_joystick_y1 > 680) angulo3 +=10;

  //comparaciones control 2
  if(valor_joystick_y2 < 340) angulo4 -=10;
  else if(valor_joystick_y2 > 680) angulo4 +=10;
 
  if(valor_joystick_x2 < 340) angulo5 -=10;
  else if(valor_joystick_x2 > 680) angulo5 +=10;

 
  //comparacion de los dos botones
  if(valor_B1 == 0) angulo6 -=5;
  
  if(valor_B2 == 0) angulo6 +=5;

  //comparaciones potenciometro
  if(valor_pot < 350) angulo1 -=5;
  else if(valor_pot > 720) angulo1 +=5;
 
  if(valor_joystick_x2 < 340) angulo5 -=10;
  else if(valor_joystick_x2 > 680) angulo5 +=10;
  // comparacion ambos controles al mismo tiempo
 //if(valor_joystick_x1 < 340 && valor_joystick_x2 < 340) angulo1 -=10;
 // else if(valor_joystick_x1 > 680 && valor_joystick_x2 > 680) angulo1 +=10;

  //se establecen declarar los angulos max/min.
  angulo1 = min(145, max(15, angulo1));
  angulo2 = min(175, max(5, angulo2));
  angulo3 = min(170, max(5, angulo3));
  angulo4 = min(145, max(15, angulo4));
  angulo5 = min(145, max(15, angulo5));
  angulo6 = min(145, max(15, angulo6));

  //movimiento de los servos
  antebrazo.write(angulo2);
  brazo.write(angulo3);
  base.write(angulo1);
  garra.write(angulo6);
  muneca.write(angulo4);
  giromuneca.write(angulo5);
  Serial.println(valor_pot);
  delay(100);

  


}