OBSTÁCULO evitando BOT (2 / 2 paso)

Paso 2: Escribir código

void setup() {}
Serial.Begin(9600);
pinMode(trigPin,OUTPUT);
pinMode(a,OUTPUT);
pinMode(b,OUTPUT);
pinMode(c,OUTPUT);
pinMode(d,OUTPUT);
pinMode(echoPin,INPUT);
}

flotador meaSure()
{

digitalWrite(trigPin,LOW);
delayMicroseconds(2000);

digitalWrite(trigPin,HIGH);
delayMicroseconds(10);

digitalWrite(trigPin,LOW);
pingTime=pulseIn(echoPin,HIGH);
distancia =(speedOfSound*pingTime)/20000; //in cm
volver a distancia;
}

{} void forward()

digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
Delay(500);
Stopp();

}
void backward() {}
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
Delay(1000);
Stopp();

}
left() vacío {}
digitalWrite(a,HIGH);
digitalWrite(b,LOW);
digitalWrite(d,HIGH);
digitalWrite(c,LOW);
Delay(570);
Stopp();

}
right() vacío {}
digitalWrite(b,HIGH);
digitalWrite(a,LOW);
digitalWrite(c,HIGH);
digitalWrite(d,LOW);
Delay(1000);
Stopp();

}
void stopp() {}
digitalWrite(a,LOW);
digitalWrite(b,LOW);
digitalWrite(c,LOW);
digitalWrite(d,LOW);
Serial.println("Stopp");
}

void loop() {}
Mida la distancia
digitalWrite(trigPin,LOW);
delayMicroseconds(2000);

digitalWrite(trigPin,HIGH);
delayMicroseconds(10);

digitalWrite(trigPin,LOW);
pingTime=pulseIn(echoPin,HIGH);
distancia =(speedOfSound*pingTime)/20000; //in cm
if(Distance>20) {}
Forward();
Serial.println ("No obstackle");
}
Si (distancia < = 20) {}
Stopp();
Serial.println ("comprobación de izquierda");
Left();
Delay(250);

lDistance = meaSure();
Serial.println ("comprobación de derecha");
Right();
Delay(250);
rDistance=meaSure();
Si (rDistance > lDistance & & rDistance > 20) {}
Forward();
Serial.println ("avanzar en el camino correcto");
}
Si (rDistance < = lDistance & & lDistance > 20) {}
Right();
Delay(250);
Forward();
Serial.println ("avanzar en el camino izquierdo");
}
Else {}
Serial.println ("en cueva");
Serial.println ("volver y comprobar una vez más);
backward();
Stopp();
Left();
Delay(250);
lDistance=meaSure();
Right();
Delay(250);
rDistance=meaSure();
Si (rDistance > lDistance & & rDistance > 20) {}
Forward();
}
Si (rDistance < = lDistance & & lDistance > 20) {}
Right();
Delay(250);
Forward();
}
}
}
}