#include <Servo.h>
Servo MI;
Servo MD;
int medida;
bool sd, st;


void setup() {
  Serial.begin (9600);
  pinMode(2,INPUT);  // Echo a pin2
  pinMode(3,OUTPUT); // Trigger pin3
  pinMode(4,INPUT); //infrarrojo delantero
  pinMode(5,INPUT); //infrarrojo trasero
  ////////////////////////////////////////////////
  delay(4000);  // Espero 5 segundos
  MI.attach(10);
  MD.attach(9);
  while( !digitalRead(5)){      // Mientras el 
    MI.write(0);                  //sensor trasero
    MD.write(180);                // no detecte negro
  } 
   MI.write(180);                  //sensor trasero
   MD.write(0);
  delay(2000);
  /////////////////////////////////////////////////////
}

void loop() {
  Serial.println(medida);
  sd = digitalRead(4);
  st = digitalRead(5);
  if(sd) retrocede();
  if(st) avance();
  medida=medir();
  if (medida<50)avance();
  if (medida>50)gira();
  }

int medir()
{
  delay(100);
  digitalWrite(3, LOW);
  delayMicroseconds(2);
  digitalWrite(3, HIGH);
  delayMicroseconds(10);
  digitalWrite(3, LOW);
  unsigned long pulso = pulseIn(2, HIGH);
  int cm = pulso / 29 / 2;
  return(cm);
}

void gira()
{
MI.write(180);
MD.write(180);
}

void avance()
{
MI.write(180);
MD.write(0);
}
void retrocede(){
  MI.write(0);
  MD.write(180);
}