#include <Servo.

h> case 'q': //abrir pinza

char dato; {
while(Serial.read()!='s')
Servo Cintura, Hombro1, Codo, Pinzas; {
int pcintura, phombro1, pcodo, ppinzas, if(ppinzas>=175)
contador=0; ppinzas=175;
if(ppinzas<=180 && ppinzas>=0)
void setup() {
{
ppinzas++;
Cintura.attach(2); Pinzas.write(ppinzas);
Hombro1.attach(3); Serial.println(ppinzas);
Codo.attach(4); delay(15);
Pinzas.attach(5); }
}
Serial.begin(9600); break;
} }
void loop()
{ case 'p': //cerrar pinza
{
if(contador==0) while(Serial.read()!='s')
{ {
//Leer posicion inicial de cada servo para if(ppinzas<=5)
evitar movimientos bruscos ppinzas=5;
pcintura=Cintura.read(); if(ppinzas<=180 && ppinzas>=0)
phombro1=Hombro1.read(); {
pcodo=Codo.read(); ppinzas--;
ppinzas=Pinzas.read(); Pinzas.write(ppinzas);
Serial.println(ppinzas);
delay(15);
//Escribir la posicion en cada servo }
Cintura.write(pcintura); }
Hombro1.write(phombro1); break;
Codo.write(pcodo); }
Pinzas.write(ppinzas);

contador++;
} //control codo
case 'o': //arriba codo
dato='s'; {
dato = Serial.read(); while(Serial.read()!='s')
switch(dato) {
{ if(pcodo>=175)
//control pinza pcodo=175;
 if(pcodo<=180 && pcodo>=0) }
{ break;
}
pcodo++;
Codo.write(pcodo); case 'i': //abajo hombro
Serial.println(pcodo); {
delay(15); while(Serial.read()!='s')
} {
} if(phombro1<=5)
break; phombro1=5;
} if(phombro1<=180 && phombro1>=0)
{
case 'n': //abajo codo phombro1--;
{ Hombro1.write(phombro1);
while(Serial.read()!='s') Serial.println(phombro1);
{ delay(15);
if(pcodo<=5) }
pcodo=5; }
if(pcodo<=180 && pcodo>=0) break;
{ }
pcodo--;
Codo.write(pcodo); //control cintura
Serial.println(pcodo); case 'f': //derecha cintura
delay(15); {
} while(Serial.read()!='s')
} {
break; if(pcintura>=175)
} pcintura=175;
if(pcintura<=180 && pcintura>=0)
//control hombro1 {
case 'h': //arriba hombro
{ pcintura++;
while(Serial.read()!='s') Cintura.write(pcintura);
{ Serial.println(pcintura);
if(phombro1>=175) delay(15);
phombro1=175; }
if(phombro1<=180 && phombro1>=0) }
{ break;
}
phombro1++;
Hombro1.write(phombro1); case 'g': //izquierda cintura
Serial.println(phombro1); {
delay(15); while(Serial.read()!='s')
} {
 if(pcintura<=5)
pcintura=5;
if(pcintura<=180 && pcintura>=0)
{
pcintura--;
Cintura.write(pcintura);
Serial.println(pcintura);
delay(15);
}
}
break;
}

}
}