evasor de obstaculos CODIGO

 const int echo=2;

const int trig=3;

volatile unsigned distancia;

const int cerca=15;

const int duracionMaxPulso=5000;

const int ENA=11;

const int IZQ1=9;

const int IZQ2=8;

const int DER2=7;

const int DER1=6;

const int ENB=10;

void setup(){

  pinMode(trig, OUTPUT);

  pinMode(echo, INPUT);

  pinMode(ENA, OUTPUT);

  pinMode(IZQ1, OUTPUT);

  pinMode(IZQ2, OUTPUT);

  pinMode(DER2, OUTPUT);

  pinMode(DER1, OUTPUT);

  pinMode(ENB, OUTPUT);

}

void loop() {

  medir();

  if (distancia < cerca){

    obstaculo();

  }

  else{

  }

  adelante();

}

void medir(){

  digitalWrite(trig, LOW);

  delayMicroseconds(2);

  digitalWrite(trig, HIGH);

  delayMicroseconds(10);

  digitalWrite(trig, LOW);

  distancia=(pulseIn (echo, HIGH, duracionMaxPulso))/29.12/2;

  if(distancia==0) { distancia=duracionMaxPulso;}

}

void adelante(){

    digitalWrite (IZQ1, 1);

    digitalWrite (IZQ2, 0);

    digitalWrite (DER2, 0);

    digitalWrite (DER1, 1);

    digitalWrite (ENA, 90);

    digitalWrite (ENB, 90);

}

void derecha(){

  digitalWrite (IZQ1, 1);

  digitalWrite (IZQ2, 0);

  digitalWrite (DER2, 1);

  digitalWrite (DER1, 0);

  digitalWrite (ENA, 150);

  digitalWrite (ENB, 200);

}

void detenido(){

  digitalWrite (IZQ1, 0);

  digitalWrite (IZQ2, 0);

  digitalWrite (DER2, 0);

  digitalWrite (DER1, 0);

  digitalWrite (ENA, 0);

  digitalWrite (ENB, 0);

}

void obstaculo(){

  detenido();

  delay(300);

  derecha();

  delay(230);

  detenido();

  delay(100);

}