#include <Otto.h>
Otto Otto;
int dato; //variable dato para guardar el mensaje recibidque va a recibir el arduino

#define LeftLeg 2 // left leg pin, servo[0]
#define RightLeg 3 // right leg pin, servo[1]
#define LeftFoot 4 // left foot pin, servo[2]
#define RightFoot 5 // right foot pin, servo[3]
#define Buzzer 13 //buzzer pin

long ultrasound_distance_simple() {
   long duration, distance;
   digitalWrite(8,LOW);
   delayMicroseconds(2);
   digitalWrite(8, HIGH);
   delayMicroseconds(10);
   digitalWrite(8, LOW);
   duration = pulseIn(9, HIGH);
   distance = duration/58;
   return distance;
}


void setup() {
  Serial.begin(9600); //Iniciar comunicacion serila a 9600 baudios
  Serial.println("Conectado..."); //Imprimiendo el mensaje: Conectado...
  Otto.init(LeftLeg, RightLeg, LeftFoot, RightFoot, true, Buzzer);
Otto.home();

  pinMode(8, OUTPUT);
  pinMode(9, INPUT);
    Otto.home();
  Otto.sing(S_superHappy);
  delay(1*1000);

}

void loop() {
  if(Serial.available()>0){  //si la señal serial es mayor que cero
    dato=Serial.read(); //guardar en la variable dato el mensaje recibido por el puerto serial
  }

  if(dato=='a'){
    sendAck(); //inicio de un comando
    Otto.walk(1,10000,1); // FORWARD
    sendFinalAck(); //final de un comando
  }
  if(dato=='b'){
    sendAck();
    Otto.init(LeftLeg, RightLeg, LeftFoot, RightFoot, true, Buzzer);
    Otto.home();
    sendFinalAck();
  }
  if(dato=='c'){
    sendAck();
    Otto.walk(1,1000,-1); // BACKWARD
    sendFinalAck();
  }
}

void sendAck() {
  delay(30); 
  Serial.print(F("&&"));
  Serial.print(F("A"));
  Serial.println(F("%%"));
  Serial.flush(); 
}

void sendFinalAck() {
  delay(30);
  Serial.print(F("&&"));
  Serial.print(F("F"));
  Serial.println(F("%%"));
  Serial.flush();
}


/*#include <Servo.h>
Servo leg_izq, leg_der, pie_izq, pie_der;
int buz=13;
int t=1000, tb=75;
int dato;
void setup(){
  Serial.begin(9600);
  leg_izq.attach(2, 544, 2400);
  leg_der.attach(3, 544, 2400);
  pie_izq.attach(4, 544, 2400);
  pie_der.attach(5, 544, 2400);
  pinMode(buz, OUTPUT);
  inicio();
  delay(1000);
  pitido();
  Serial.println("Conectado...");
}

void loop(){
  if(Serial.available()>0){
    dato=Serial.read();
  }
  if(dato=='w'){
    leg_izq.write(45);
    leg_der.write(45);
    delay(t);
    leg_izq.write(135);
    leg_der.write(135);
    delay(t);
    //pie_izq.write(90);
    //pie_izq.write(90);
  }
}

void inicio(){
  leg_izq.write(90);
  leg_der.write(90);
  pie_izq.write(90);
  pie_izq.write(90);
}

void pitido(){
  for(int k=0; k<4; k++){
    tone(buz,440);
    delay(tb);
    noTone(buz);
    delay(tb);
  }
}*/