Test Arduino

#include <Servo.h>


/*
 * sw pulsa gnd
 */

//A0 giro brazo servo1
//A1 LATERAL A servo2 elevacion pinza
//A2 LATERAL B servo3
//A3 PINZA servo4
Servo servo1; Servo servo2;Servo servo3; Servo servo4;
Servo servo_n[4]={servo1,servo2,servo3,servo4};

int vrx1,vry1;vrx2,vry2;//Lectura sensores analogicos

/********************************************************************************/
void imprime(char* cadena,int v){//Funcion impresion un string y un valor decimal
        Serial.println(cadena);
        Serial.print(v,DEC);
        Serial.println();
}
/********************************************************************************/
void pinzaServo(int servo){//Control pinza 90 - 120 grados
    for (int bucle=0;bucle<4;bucle ++){//Repite x veces
      for (int grados=90 ;grados<=130;grados+=5){
      servo_n[servo].write(grados);
        delay(500);//Retardo 1seg.
    }
  }

  servo_n[servo].write(10);

}
/********************************************************************************/
void autoServo(int servo){//Movimiento automatico de un servo
  for (int grados=0 ;grados<=180;grados+=10){
    servo_n[servo].write(grados);
      delay(1000);//Retardo 1seg.
  }

}
/********************************************************************************/
void conectaServos(){//Conecta todos los servos
  servo1.attach(14); //A0 analog pin 0
  servo2.attach(15); //A1 analog pin 1
  servo3.attach(16); //A1 analog pin 2
  servo4.attach(17); //A1 analog pin 3
}

/********************************************************************************/
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
}
/********************************************************************************/
// the setup function runs once when you press reset or power the board
void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(1,OUTPUT);//LED

  servo1.attach(14); //A0 analog pin 0
  //servo1.setMaximumPulse(2000);
  //servo1.setMinimumPulse(700);

  servo2.attach(15); //A1 analog pin 1
  servo3.attach(16); //A1 analog pin 2
  servo4.attach(17); //A1 analog pin 3

  Serial.begin(19200);
  Serial.println(" Preparado ");

 // Servo servo_n[2]={servo1,servo2};

}

// the loop function runs over and over again forever
void loop() {

static int v = 0;
/*
  digitalWrite(LED_BUILTIN, HIGH);   // turn the LED on (HIGH is the voltage level)
  delay(1000);                       // wait for a second
  digitalWrite(LED_BUILTIN, LOW);    // turn the LED off by making the voltage LOW
  delay(1000);                       // wait for a second
*/
  if ( Serial.available()) {//Si hay datos en el serie los lee , lectura grados servo y accion
    char ch = Serial.read();//lee un caracter 0..9 ,s,w,d,a

    switch(ch) {
      case '0'...'9'://Compone la posicion del servo en grados
        v = v * 10 + ch - '0';
        Serial.println ("Lectura ch");
        Serial.print(ch);
        break;

        //Giro del brazo 110 centro 0 90
      case 's'://Servo conectado en A0 servo1
       // servo1.write(v);//Envia el valor de grados al servo
        servo_n[0].write(v);
        imprime ("v=",v);//Debug servo
        v = 0;
        break;

      case 'w':
             imprime ("sube baja ",v);//Debug servo
        servo_n[1].write(v);//Eleva baja la pinza 0 alto 110 bajo
        servo_n[2].write(v);// 110 subiendo .... abajo
        v = 0;
        break;


      case 'r'://Movimiento automamtico servo
        imprime (" AUTOMATICO ",0);
        autoServo(0);
        v=0;
        break;


      case 'p'://Mueve pinza
        imprime (" pinza ",3);
        pinzaServo(3);
        break;


      case 'd'://Desconecta
        desconectaServos();
        break;

      case 'a'://Conecta
        conectaServos();
        break;

    }
  }






}