#include <Servo.h>
Servo servo1;
Servo servo2;
Servo servo3;

// Pines y variables


int angulo1 = 90;
int angulo2 = 90;
int angulo3 = 90;

const int SW_pin = 2; // Pin entrada digital botón
const int X_pin = 0; // Pin analógico X
const int Y_pin = 1; // Pin analógico Y
const int button = 13; // Pin Reset
const int servo1Pin = 7; // Pin Servo 1
const int servo2Pin = 6; // Pin Servo 2
const int servo3Pin = 5; // Pin Servo 3
 

 //Setup de los pines como entrada o salida
void setup() {
  
 servo1.attach(servo1Pin);
 servo2.attach(servo2Pin);
 servo3.attach(servo3Pin);
 pinMode(button, INPUT);
digitalWrite(SW_pin, HIGH);

 
}

void loop() {

   
   
// Coódigo Servos//
// Servo 1
if (digitalRead (SW_pin) == 1){
while (analogRead (X_pin) < 450){
angulo1 = servo1.read();
angulo1 = angulo1+1;
if (angulo1 > 160){
  angulo1 = 160;
}
servo1.write(angulo1);
delay (10);
}

while (analogRead (X_pin) >550){
angulo1 = servo1.read();
angulo1 = angulo1-1;
if (angulo1 < 20){
  angulo1 = 20;
}
servo1.write(angulo1);
delay(10);
}
}


// Servo 2
while (analogRead (Y_pin) <450){
angulo2 = servo2.read();
angulo2 = angulo2+1;
if (angulo2 > 160){
  angulo2 = 160;
}
servo2.write(angulo2);
delay (10);
}

while (analogRead (Y_pin) >550){
angulo2 = servo2.read();
angulo2 = angulo2-1;

if (angulo2 < 20){
  angulo2 = 20;
}
servo2.write(angulo2);
delay(10);
}

// Servo 3
if (digitalRead (SW_pin) == 0){
while (analogRead (X_pin) <450){
angulo3 = servo3.read();
angulo3 = angulo3+1;
if (angulo3 > 160){
  angulo3 = 160;
}
servo3.write(angulo3);
delay (10);


}

while (analogRead (X_pin) >550){
angulo3 = servo3.read();
angulo3 = angulo3-1;
if (angulo3 < 20){
  angulo3 = 20;
}
servo3.write(angulo3);
delay(10);
}

}


if (digitalRead(button) == HIGH){
int angulo1 = 90;
servo1.write(angulo1);
int angulo2 = 90;
servo2.write(angulo2);
int angulo3 = 90;
servo3.write(angulo3);
}

}