Brazo Robot Servo controlado por ARDUINO UNO R3

//Antonio Villanueva Segura Control Brazo robotizado con 4 servos y 2 Joysticks ...
#include <Servo.h>

//Define PINS
//A0-A1-A2-A3 Lectura Joystick
#define A0 14
#define A1 15
#define A2 16
#define A3 17
#define BOTON1  9
#define BOTON2  8

//Servos definiciones alternativas mucho mas claras
//Giro Brazo
#define GIRO 18

//Lateral A A5
#define LATA 19

//Lateral B
#define LATB 5

//Pinza
#define PINZA 6

//Declaro 4 servos
Servo servo1; Servo servo2;Servo servo3; Servo servo4;

//Array de los 4 servos anteriores para acceso por bucle ..
Servo servo_n[4]={servo1,servo2,servo3,servo4};

int vrx1,vry1,vrx2,vry2;//Lectura sensores analogicos A0 A1 A2 A3

/********************************************************************************/

void imprime(char* cadena,int v){//Funcion impresion un string y un valor decimal
        Serial.print(cadena);
        Serial.print(v,DEC);
      //  Serial.println();
}

/********************************************************************************/

void pinzaServo(bool cierra){//Control pinza 90 - 120 grados control por boton

  if (cierra){//CIERRA
    for (int grados=130 ;grados>=10;grados-=5){//Cierra lentamente
        servo_n[3].write(grados);

        delay(500);//Retardo 1seg.
    }
  }
  else {//ABRE
    for (int grados=90 ;grados<=130;grados+=5){//Abre lentamente
        servo_n[3].write(grados);
        delay(500);//Retardo 1seg.
      }
   }
}

/********************************************************************************/
void pinzaServoGrados(int grados){//Control pinza por grados 90 -120 grados
  servo_n[3].write(grados);
}
/********************************************************************************/
void Giro(int grados){//Giro del brazo segun posicion joystic
  servo_n[0].write(grados);
}

/********************************************************************************/
void Laterales(int grados , int gradosb){//Giro del brazo segun posicion joystic
  servo_n[1].write(grados);
  servo_n[2].write(gradosb);
}
/********************************************************************************/
void conectaServos(){//Conecta todos los servos
  servo1.attach(GIRO); //Giro A4
  servo2.attach(LATA); //Lateral A A5
  servo3.attach(LATB); //Lateral B PD5
  servo4.attach(PINZA); //Pinza PD6
}

/********************************************************************************/
void desconectaServos(){//Desconectta los servos
  servo1.detach();; //A0 analog pin 0
  servo2.detach(); //A1 analog pin 1
  servo3.detach(); //A1 analog pin 2
  servo4.detach(); //A1 analog pin 3
}
/********************************************************************************/
//La funcion setup arranca al inicio o en RESET
void setup() {

  // initialize digital pin LED_BUILTIN as an output.
    pinMode(LED_BUILTIN, OUTPUT);
   pinMode(1,OUTPUT);//LED

  //Click Joystick
  pinMode(BOTON1 , INPUT_PULLUP);   //activar resistencia pull up
  pinMode(BOTON2 , INPUT_PULLUP);   //activar resistencia pull up

//Servos
  conectaServos(); //Conecta todos los servos A4,A5,PD5,PD6

  Serial.begin(19200);
  Serial.println("BRAZO ICARO PREPARADO  ");

}
/********************************************************************************/

/********************************************************************************/
// Bucle principal del ARDUINO UNO R3
void loop() {
  //delay(100);//Pausa entre lecturas analogicas


//Reescalo los valores de los sensores potenciometro analogRead produce un valor entre 0 y 1024 que traduzco a grados de 0 a 180
//map(value, fromLow, fromHigh, toLow, toHigh)
 vrx1 = map(analogRead(A0), 0, 1024, 40, 110);   // cambiar escala de 0 a 180 grados GIRO
 vry1 = map(analogRead(A1), 0, 1024, 60, 100);   // cambiar escala de 0 a 180 grados LATA
 vrx2 = map(analogRead(A2), 0, 1024, 60, 100);   // cambiar escala de 0 a 180 grados LATB
 vry2 = map(analogRead(A3), 0, 1024, 90, 120);   // cambiar escala de 90 a 120 grados PINZA
//pinzaServo(digitalRead(BOTON1));
  Giro (vrx1);//Giro brazo
  pinzaServoGrados(vry2);
  Laterales(vry1 , vrx2);
  imprime("vrx1 = ",vrx1);// lee 0 600
  imprime("\t vry1 = ",vry1);
  imprime("\t BOTON = ",digitalRead(BOTON1));
  imprime("\tvrx2 = ",vrx2);
  imprime("\tvry2 = ",vry2);
  imprime("\t BOTON = ",digitalRead(BOTON2));
  Serial.println();

}