#include <Servo.h>

Servo servomotor[3];
int servoPin[] = {11, 10, 9};

int buttonPin1 = 13;
int buttonPin2 = 12;
bool buttonState;
bool lastButtonState;
bool buttonPressed[] = {1, 0};

//Angulos
int servoAngle[] = {0, 0};
bool backWay;
int turnSpeed[] = {4, 8};

//Tiempo
unsigned long clockMill;
unsigned long lastTime[3];

void setup() {
  for(int i=0; i<3; i++) {
    servomotor[i].attach(servoPin[i]);
    servomotor[i].write(servoAngle[i]);
  }
  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT);
  delay(500); 
}

void loop() {
  clockMill = millis();
  buttonState = digitalRead(buttonPin1);
  buttonState = digitalRead(buttonPin2);
  if(buttonState != lastButtonState) {
    if(!lastButtonState) {
      for(int i=0; i<3; i++) {
        buttonPressed[i] = !buttonPressed[i];
      }
    } 
    lastButtonState = buttonState;
  }

  for(int i=0; i<3; i++) {
    if(clockMill - lastTime[i] >= turnSpeed[i] && buttonPressed[i]) {

      if(servoAngle[i] <= 0 || !backWay) {
        servoAngle[i]++;
        servoAngle[i] = constrain(servoAngle[i], 0, 180);
        servomotor[i].write(servoAngle[i]);
        backWay = LOW;
      }

      if(servoAngle[i] >= 180 || backWay) {
        servoAngle[i]--;
        servoAngle[i] = constrain(servoAngle[i], 0, 180);
        servomotor[i].write(servoAngle[i]);
        backWay = HIGH;
      }
      lastTime[i] = clockMill;
    }
  }
  delay(2);
}