#include <Servo.h>
Servo MI;
Servo MD;
int medida;

void setup() {
  pinMode(3,OUTPUT);
  pinMode(2,INPUT);

  MI.attach(10);
  MD.attach(9);
}

void loop() {
  medida=medir();
  if (medida<40)avance();
  if (medida>40)gira();
  }

int medir()
{
  delay(200);
  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(0);
MD.write(0);
}

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