/ Define the Servo Class
#include <Servo.h>
Servo PieDerServo,MusloDerServo;
Servo PieIzqServo,MusloIzqServo;

// References Positions
#define CentroPosPieDer   90  
#define CentroPosPieIzq   90  
#define CentroPosMusloDer 90  
#define CentroPosMusloIzq 90  
#define MaximaPos         30
#define MinimaPos         -30

// Variables to control
int velocidad=20;
byte PieDerPos=CentroPosPieDer,MusloDerPos=CentroPosMusloDer;
byte PieIzqPos=CentroPosPieIzq,MusloIzqPos=CentroPosMusloIzq;
byte dato_serie;
bool estado = LOW;

// Setup Function to set the config and the initial values
void setup() {
  // Begin the Servos
  PieDerServo.attach(5);
  PieIzqServo.attach(4);
  MusloDerServo.attach(3);
  MusloIzqServo.attach(2);
  PieDerServo.write(PieDerPos);
  MusloDerServo.write(MusloDerPos);
  PieIzqServo.write(PieIzqPos);  
  MusloIzqServo.write(MusloIzqPos);
  // Begin the Bluetooth
  Serial.begin(115200);  
}
void loop() {
  // Secuence to move the right side
  if (estado){
    for (MusloDerPos = CentroPosMusloDer; MusloDerPos <= (CentroPosMusloDer + MaximaPos); MusloDerPos += 1) {
      MusloDerServo.write(MusloDerPos);
      delay(velocidad);
    }
    for (PieDerPos = CentroPosPieDer; PieDerPos <= (CentroPosPieDer + MaximaPos); PieDerPos += 1) {
      PieDerServo.write(PieDerPos);
      PieIzqServo.write(PieDerPos);
      delay(velocidad);
    }
    for (MusloDerPos = (CentroPosMusloDer + MaximaPos); MusloDerPos >= CentroPosMusloDer; MusloDerPos -= 1) {
      MusloDerServo.write(MusloDerPos);
      delay(velocidad);
    }
    for (PieDerPos = (CentroPosPieDer + MaximaPos); PieDerPos >= CentroPosPieDer; PieDerPos -= 1) {
      PieDerServo.write(PieDerPos);
      PieIzqServo.write(PieDerPos);
      delay(velocidad);
    }  
  }
  // Secuence to move the left side
  if (estado){
    for (MusloIzqPos = CentroPosMusloIzq; MusloIzqPos >= (CentroPosMusloIzq + MinimaPos); MusloIzqPos -= 1) {
      MusloIzqServo.write(MusloIzqPos);
      delay(velocidad);
    }
    for (PieIzqPos = CentroPosPieIzq; PieIzqPos >= (CentroPosPieIzq + MinimaPos); PieIzqPos -= 1) {
      PieDerServo.write(PieIzqPos);
      PieIzqServo.write(PieIzqPos);
      delay(velocidad);
    }
    for (MusloIzqPos = (CentroPosMusloIzq + MinimaPos); MusloIzqPos <= CentroPosMusloIzq; MusloIzqPos += 1) {
      MusloIzqServo.write(MusloIzqPos);
      delay(velocidad);
    }
    for (PieIzqPos = (CentroPosPieIzq + MinimaPos); PieIzqPos <= CentroPosPieIzq; PieIzqPos += 1) {
      PieDerServo.write(PieIzqPos);
      PieIzqServo.write(PieIzqPos);
      delay(velocidad);
    }  
  }
}
void serialEvent(){
  // Serial event
  while (Serial.available()){
    dato_serie = Serial.read();
    switch(dato_serie){
      // Case the data is Turn ON
      case 255:
        estado = HIGH;
        Serial.println("ON");
      break;
      // Case the data is Turn OFF
      case 0:
        estado = LOW;
        Serial.println("OFF");
      break;
      // Case the data is Change Velocity
      default:
        velocidad = map(dato_serie,1,254,8,30);
        Serial.println(velocidad);
      break;     
    }
  }  
}
// End the Program