Cdigo para controlar brazo robot de 4 grados de libertad con arduino

#include <Servo.h>//incluir la libreria de servo motores

Servo
Servo
Servo
Servo

myservo1;//SERVO
myservo2;//SERVO
myservo3;//SERVO
myservo4;//SERVO

1
2
3
4

ROTACION
AD
WS
PINZA

String dato;
String aux;

int
int
int
int
int
int
int
int
int
int

a1;
d1;
w1;
s1;
q1;
e1;
z1;
x1;
inc=1;//incremento en grados, inicial en 1 grado.
pos =0;//variable contador posicion inicial del brazo

void setup() {
Serial.begin(9600);//iniciar comunicacion serial, (velocidad den baudios)
myservo1.attach(3);//SERVO
myservo2.attach(5);//SERVO
myservo3.attach(6);//SERVO
myservo4.attach(9);//SERVO

ROTACION asignar al pin 3 el objeto myservo 1

AD
WS
PINZA

pinMode (10,OUTPUT);//velocidad 1 presicin de 1 grados

pinMode (11,OUTPUT);//velocidad 2 presicin de 5 grados
pinMode (12,OUTPUT);//velocidad 3 presicin de 9 grados
// pinMode (numero de pin, INPUT/OUTPUT) => asignar los pines digitales
como salida OUTPUT
}
void loop() {
if (pos==0){
myservo1.write(90);
myservo2.write(115);
myservo3.write(136);

myservo4.write(66);
pos=pos+1;
//myservo1.write(angulo al que deseas mandar) => mandar los motores
a la posicion inicial adecuada.
}
//codigo para incidar la velocidad actual de movimiento o la presicion de
movimiento del servo
if (inc==1){
digitalWrite(10, HIGH);//digital write => cambiar el estado de la salida del
arduino HIGH-ALTO 5V, LOW BAJO 0V
digitalWrite(11, LOW);
digitalWrite(12, LOW);
}
if (inc==5){
digitalWrite(10, LOW);
digitalWrite(11, HIGH);
digitalWrite(12, LOW);
}
if (inc==9){
digitalWrite(10, LOW);
digitalWrite(11, LOW);
digitalWrite(12, HIGH);
}
if (Serial.available()>0){//si existe comunicacion serial o un dato recibido
diferente de 0
char c = Serial.read();//leer del puerto serial y guardar en char c
aux+=c;
dato= aux;
aux="";
}
a1=myservo1.read();//leer la posicion actual del servo
d1=myservo1.read();
w1=myservo2.read();
s1=myservo2.read();
q1=myservo3.read();
e1=myservo3.read();
z1=myservo4.read();
x1=myservo4.read();
if (dato.length ()>0){
if (dato=="c"){
if (inc<10){
inc= inc + 4;
dato="";
}
if (inc>10){
inc=1;

dato="";
}
}
if (dato=="a"){
if(a1<170){
myservo1.write(a1+inc);
dato="";
}
}
if (dato=="d"){
if(d1>10){
myservo1.write(d1-inc);
dato="";
}
}
if (dato=="w"){
if(w1<177){
myservo2.write(w1+inc);
dato="";
}
}
if (dato=="s"){
if(s1>54){
myservo2.write(s1-inc);
dato="";
}
}
if (dato=="q"){
if(q1<174){
myservo3.write(q1+inc);
dato="";
}
}
if (dato=="e"){
if(e1>98){
myservo3.write(e1-inc);
dato="";
}
}
if (dato=="z"){
if(z1<64){
myservo4.write(z1+inc);
dato="";
}
}
if(dato=="x"){
if(x1>20){
myservo4.write(x1-inc);

dato="";
}
}