Paso 5: Implementar el algoritmo de "La mano izquierda en la pared" en el código de Arduino
Una vez que tenemos la función de readLFSsensors() modificada para incluir los sensores extra 2, podemos volver a escribir la función de bucle para ejecutar el algoritmo, como se describe en el último paso:
void loop()
{
readLFSsensors();
interruptor (modo)
{
caso NO_LINE:
motorStop();
goAndTurn (a la izquierda, 180);
rotura;
caso CONT_LINE:
runExtraInch();
readLFSsensors();
Si (modo == CONT_LINE) mazeEnd();
otro goAndTurn (a la izquierda, 90); / / o es una "T" o "Cruz"). En ambos casos, va a la izquierda
rotura;
caso RIGHT_TURN:
runExtraInch();
readLFSsensors();
Si (modo == NO_LINE) goAndTurn (la derecha, 90);
rotura;
caso LEFT_TURN:
goAndTurn (a la izquierda, 90);
rotura;
caso FOLLOWING_LINE:
followingLine();
rotura;
}
}
Algunas funciones de nuevo aparece aquí.
followingLine() es el mismo utilizado con el Robot de línea siguiente, donde si sólo está siguiendo una línea, debe calculatePID(); y control de los motores dependiendo de los valores de PID: motorPIDcontrol();
runExtraInch(): Empujará el robot hacia adelante un poco. Cuánto el robot ejecutará dependerá del tiempo que empleas en la función de retardo, antes te mando el motor se detenga.
void runExtraInch(void)
{
motorPIDcontrol();
Delay(extraInch);
motorStop();
}
goAndTurn (dirección, ángulo): Esta función especial es importante, porque no se puede dar el robot, pronto te das cuenta el tipo de intersección que es. Recuerde que nos proyecta un Robot diferencial que al hacer giros, "gira alrededor de su hacha" y por lo tanto, para mover a 90o y seguir la línea de forma continua, el centro de las ruedas debe estar alineado con el centro de intersección. Una vez que la línea de los censores están por delante de su eje, debe ejecutar el robot hacia adelante para alinearlos. La constante de tiempo adjGoAndTurn se debe ajustar dependiendo de la distancia entre la línea axe y sensor ("d"), velocidad y tamaño de las ruedas (véase el cuadro de ilustración).
void goAndTurn (int direccion, int grados)
{
motorPIDcontrol();
Delay(adjGoAndTurn);
motorTurn (dirección, grados);
}
En este momento el robot es de hecho "para resolver un laberinto"! Apenas termine el "primer paso". No importa donde empezar dentro de un laberinto, hasta llegar al final.
A continuación, una prueba de esta fase del proyecto:
