Robot de laberinto

[Lorenzo M. Oliver]

Este documento trata de describir el proceso que seguí para construir un robot capaz de salir de un laberinto, en el VI Campeonato de Euskadi de microbótica, organizado por la Universidad de Deusto.

Copyright (c) 2007 Xabier Ugarte Pedrero
Este documento, así como el código fuente contenido en él, está protegido bajo la “Licencia Reconocimiento-No comercial-Compartir bajo la misma licencia 2.5 España License de Creative Commons”.
Usted es libre de utilizarlo, copiarlo, distribuirlo, y modificarlo, siempre que se haga bajo estos mismos términos, y bajo fines no comerciales.

Principalmente me voy a centrar en el código del programa, que en un principio iba a ser escrito en lenguaje ensamblador de PIC, para ser grabado en un PIC16F876, pero por razones de tiempo, acabé utilizando el BASIC Stamp 2, de Parallax. LA PRUEBA La prueba consiste en conseguir que un microbot encuentre un objeto dentro de un laberinto, y tras ello, salga del mismo por cualquiera de las dos salidas. Es esencial tener en cuenta que el propio objeto, existe físicamente, y puede bloquear una de las salidas. El laberinto es de madera, con suelo y paredes de color blanco. Cada casilla tiene unas dimensiones de 20x20 cm. No existe limitación para las medidas o peso del robot, pero debe ser lo suficientemente pequeño como para tener libertad de giro dentro de cada casilla. Para ello, el robot puede almacenar en memoria datos acerca del laberinto, de forma que una vez en él pueda guiarse. El mapa del laberinto es conocido de antemano. La dificultad consiste en que el robot saldrá desde cualquier casilla, es decir, hasta el momento de la prueba no se conoce la casilla desde la que el robot partirá, ni la posición del objeto dentro del laberinto. El robot no se puede reprogramar, pero sin embargo se permite introducirle mediante pulsadores, interruptores, R.F., infrarrojos, las dos posiciones, así como su orientación. ANALISIS DEL MAPA DEL LABERINTO En esta imagen podemos observar como es el mapa del laberinto. Como podemos observar, el laberinto tiene dos salidas, cualquiera de las dos totalmente válidas. Existe un único camino directo entre ellas, es decir, ambas estás unidas directamente, y en dicho camino, existen desviaciones que acaban en ramas sin salida. Para ir de una salida a otra, hay un camino obligatorio que debemos seguir, es decir, una sucesión de casillas, o como más tarde llamaremos, nodos, por los que debemos pasar necesariamente. Además, podemos recorrer algunas de las ramas, que harán mas largo el recorrido. A partir de ahora, para buscar el camino dentro del laberinto, en vez de centrarnos en las paredes que no podemos atravesar, nos centraremos en los caminos, como lineas que atraviesan el centro de cada casilla. A partir de este momento, y mediante la observación, podemos concluir que este conjunto de caminos, no es más que una sencilla estructura de árbol, compuesto por nodos (cada casilla) y enlaces entre los nodos. Ahora podemos ver marcado el camino que seguiríamos para ir de una salida a otra, recorriendo todas las casillas del laberinto, y si borramos las lineas rojas que nos llevan a ramas sin salida, dejamos solo el camino directo, y por tanto más corto, y mas eficiente. Nuestro algoritmo trata de encontrar el camino mas corto, no solo desde una salida hasta la otra, sino desde cualquier punto del árbol, hasta cualquier otro punto, en cualquiera de los dos sentidos posibles del recorrido. Aquí tenemos el árbol que habíamos mencionado, estirado, y ordenado, de una manera especial. Las dos salidas son la 4 y la 33.

Cada nodo, proviene de un solo padre, y puede tener 1 o 2 hijos. El orden elegido es el de inorden. Es una numeración recursiva, y se basa en los siguientes pasos: Numeramos la raíz. Numeramos el subárbol que cuelga de uno de sus hijos. Numeramos el subárbol que cuelga de su otro hijo (si es que tiene). Esta numeración nos conviene a la hora de confeccionar el algoritmo, ya que si lo analizamos, nos damos cuenta de que los descendientes de cada nodo, son una sucesión de números comprendidos entre dos límites. Es decir, si el nodo que buscamos esta entre esos dos limites, significa que está en esa rama, y continuaremos avanzando por ella. Para entender mejor esto, vemos un ejemplo: Imaginemos que estamos en el nodo 13, y queremos ir al 19. El nodo 13 tiene un solo hijo, y todos sus descendientes están comprendidos entre el 14 y el 17. Como nuestro destino (19) no está comprendido entre esos limites, significa que tenemos que avanzar hacia el padre. Ahora estamos en el nodo 12, y el proceso es similar, el 19 no está entre el 13 y el 17, así que continuamos hacia arriba. Llegamos al 11, este nodo es un poco mas complejo, ya que tiene dos hijos. Primero comprobamos uno de ellos, cuyos descendientes están entre el 12 y el 17, intervalo en el cual no está nuestro destino, sin embargo, si está en el subárbol que cuelga de su otro hijo, que comprende el intervalo entre el 18 y el 36. Así que avanzamos por esa rama. Estamos en el nodo 18, también con dos hijos, miramos los intervalos como hasta ahora y avanzamos hasta el 19. Ya estamos en nuestro destino. Como podemos comprobar, hemos avanzado a través de nodos con un solo hijo, y con dos hijos, subiendo niveles hacia arriba, a través de los padres, y hacia abajo, a través de sus hijos. También hemos trazado el camino más corto entre los dos puntos. He aquí el potencial de esta numeración. Los números que hemos especificado, no son los definitivos, pero si lo es el orden relativo. En un futuro, cada número corresponderá a una dirección de memoria, en la cual tendremos información sobre ese nodo, (Dirección del padre, de los hijos... orientaciones, etc). REPRESENTACION DE LA ORIENTACION Para representar la orientación en 2 bits, utilizamos en siguiente esquema. Así, comparando los números, 00, 01, 10, 11, podemos determinar si necesitamos girar a la izquierda o a la derecha una o varias veces, para hacer coincidir la orientación de nuestro robot con la orientación deseada. FORMATO DE LA INFORMACION DE CADA NODO Cada nodo con un solo hijo, dispondrá de la siguiente información, compuesta por 4 bytes. Imaginemos que es el primer nodo. Su información comienza en la dirección 0 de la memoria eeprom. El primer byte, solo tiene 2 bits útiles, el B7 indica si es un nodo con 1 hijo (0) o si es un nodo con dos hijos (1). El B0, nos indica si la salida más cercana a ese nodo es la 4 (según la numeración que hemos indicado antes) representado por un 0 o la 33, representada mediante un 1. El siguiente byte, tiene 6 bits para indicar la dirección del nodo que representaría el limite inferior del intervalo que hemos explicado antes (LIM1A) y OR1, de dos bits, contiene la orientación de la rama correspondiente a ese intervalo. El siguiente bytes (LIM1B), contiene el limite superior del intervalo y la misma orientación. El último byte, nos dice la dirección del nodo padre, y su orientación. Esta es la forma en la que se almacenan los datos de un nodo con 2 hijos. La única diferencia, es que ocupamos 8 bytes en lugar de 4, estando los dos últimos totalmente inutilizados. Esto es debido a que de esta manera, las direcciones son múltiplos de 4, y al poner los limites de los intervalos en cada byte, escribiremos los 6 bits de mayor peso, quedando SIEMPRE los 2 bits de menos peso a 0 (son direcciones múltiplos de 4). Aprovechamos estos dos bits para poner la orientación. Si queremos extraer solo la dirección, copiamos la información y ponemos a 0 los dos bits de orientación. Esta forma de desperdiciar un poco de memoria, nos garantiza una facilidad tremenda a la hora de escribir el algoritmo, por lo tanto, no se considera algo negativo, sino todo lo contrario. Después tenemos que codificar la información de cada nodo, bit a bit, tarea que hay que realizar con sumo cuidado, ya que un error de un solo bit puede afectar el funcionamiento del robot. Vamos situando la información en memoria, en el orden en que hemos configurado el árbol, y teniendo cuenta el numero de bytes que se reservan para cada tipo de nodo. De esta forma, el nodo 1 quedaría en la dirección 0, y a partir de ahora, su número será ese, el de su dirección. El siguiente estará en la dirección 4, después en la 8, en la 10, y así sucesivamente (tened en cuenta que usamos numeración hexadecimal). Los nodos pasarán a llamarse según la dirección de memoria en la que comienzan sus datos. El mapa del laberinto, si cambiamos los números de nodo, y volvemos a dotar al árbol con su forma original, queda de la siguiente manera:

Vamos a hacer un ejemplo, de como se codificaría la información de un nodo, y como quedaría representado en memoria. Vamos a tomar el nodo 20. Esto quiere decir que comenzaríamos a escribir estos bytes a partir de la posición 20. Sabemos que está más cerca de la salida 10, que de la 9C, luego el bit de menos peso, será 0. También tenemos en cuenta que solo tiene un hijo, así que el bit de más peso será 0. El resto están inutilizados. Primer byte: 00000000 => 0x00 El segundo y tercer byte: Sus hijos están comprendidos entre el 24 (00100100)y el AC(10101100), y para ir por esa rama, tendremos que ir por el oeste (11). Segundo byte: 00100111 => 0x27 Tercer byte: 10101111 => 0xAF El padre es el nodo 1C(00011100), y está situado al este, luego la orientación será 01. Cuarto byte: 00011101 => 0x1D Ahora tenemos que repetir esta operación con los 36 nodos. El mapa de memoria queda representado de la siguiente forma (ya escrito en PBASIC, en forma de directivas para almacenar en memoria los datos en tiempo de grabación). En el código del programa, como norma general, utilizaré notación hexadecimal. Cada linea representa una casilla o nodo, algunas con 4 bytes, y otras con 6. En cada linea especifico la dirección en la que comienza a copiarse la siguiente información. DATA @$00, $00, $04, $AC, $00 DATA @$04, $00, $09, $AD, $02 DATA @$08, $80, $16, $AE, $10, $10, $07 DATA @$10, $00, $FF, $FF, $0A DATA @$14, $00, $1A, $AE, $08 DATA @$18, $00, $1F, $AF, $14 DATA @$1C, $00, $23, $AF, $19 DATA @$20, $00, $27, $AF, $1D DATA @$24, $00, $2A, $AE, $21 DATA @$28, $00, $2E, $AE, $24 DATA @$2C, $81, $35, $49, $53, $AF, $28 DATA @$34, $01, $38, $4C, $2F DATA @$38, $01, $3D, $4D, $36 DATA @$3C, $81, $45, $45, $4A, $4E, $3B DATA @$44, $01, $FF, $FF, $3F DATA @$48, $01, $4D, $4D, $3C DATA @$4C, $01, $FF, $FF, $4B DATA @$50, $81, $60, $AC, $5B, $5F, $2D DATA @$58, $01, $5C, $5C, $51 DATA @$5C, $01, $FF, $FF, $5A DATA @$60, $01, $64, $AC, $52 DATA @$64, $81, $70, $AC, $6F, $6F, $62 DATA @$6C, $01, $FF, $FF, $65 DATA @$70, $81, $88, $AC, $79, $85, $66 DATA @$78, $01, $7D, $85, $73 DATA @$7C, $01, $80, $84, $7B DATA @$80, $01, $87, $87, $7E DATA @$84, $01, $FF, $FF, $81 DATA @$88, $81, $90, $9C, $A3, $AF, $72 DATA @$90, $01, $95, $9D, $8A DATA @$94, $01, $99, $9D, $93 DATA @$98, $01, $9D, $9D, $97 DATA @$9C, $01, $FF, $FF, $9B DATA @$A0, $81, $A8, $A8, $AE, $AE, $89 DATA @$A8, $01, $FF, $FF, $A2 DATA @$AC, $01, $FF, $FF, $A0 NUESTRO ALGORITMO Ya tenemos lo necesario para desarrollar el algoritmo. Nuestro robot en todo momento sabrá en que orientación se encuentra. Se la especificaremos inicialmente, y con cada movimiento la actualizaremos. También en todo momento conocemos en nodo en el que estamos, actualizando este dato cuando avanzamos hacia otro nodo, así como el destino al que vamos, que será en la primera fase el objeto, y en la segunda fase la salida más cercana. Además, tenemos que tener en cuenta que el objeto puede bloquear la salida, y que no podemos atravesarlo, por lo que al llegar a él, nos quedaremos en el nodo anterior, y giraremos si es necesario para encararlo, decidiendo después a que salida nos vamos a dirigir, y volviendo a repetir todo el proceso de la primera fase, para encontrar la salida.

Para entender el algoritmo, incluyo un diagrama de flujo que explica el funcionamiento general del programa.

CODIGO DEL PROGRAMA PASO A PASO

Nota: Este código no incluye un sistema para la introducción de coordenadas, necesario para realizar la prueba. Las constantes de las pausas de los movimientos, se ajustan según el nivel de pilas para que los movimientos sean lo más cercanos a 20cm en el caso de avanzar, y 90º en el caso de girar. Hay una constante de corrección al avanzar, que sirve para conseguir que el avance sea mas recto, dado que un motor gira un poco más rápido que el otro.

Además, el código del programa incluye instrucciones DEBUG, para facilitar el seguimiento y simular el comportamiento del robot en el laberinto.

Ya hemos visto antes como quedaría la información que debemos grabar en la memoria, para almacenar el mapa del laberinto, así que ahora procedemos con el resto del código. En primer lugar, tenemos la declaración de constantes del programa.

AVANZAR CON %0110
RETROCEDER CON %1001
DER CON %1010
IZQ CON %0101
QUIETO CON %0000

PAUSA_AVANZAR CON 1290
PAUSA_CO CON 6
PAUSA_IZQ CON 445
PAUSA_DER CON 445


El primer bloque corresponde a los bits que sacaremos por las patitas 03 para realizar los movimientos. El segundo, son pausas, en milisegundos, para que el robot se mueva aproximadamente 20cm al avanzar, y gire 90º. PAUSA_CO es una constante de corrección, para conseguir que el robot avance en línea recta, dado que un motor gira más rápido que el otro.

Ahora, pasamos a las variables del programa.

ORIENTACION VAR Nib
ORI VAR Nib
ORIGEN VAR Byte
DESTINO VAR Byte
NODO VAR Byte
INFO VAR Byte
LIM1A VAR Byte
LIM1B VAR Byte
LIM2A VAR Byte
LIM2B VAR Byte
PADRE VAR Byte
AUX1 VAR Byte
AUX2 VAR Byte
AUX3 VAR Byte
AUX4 VAR Byte
OBJETO VAR Byte

Empecemos con ORIENTACION y ORI, ambas de 4 bits. Solo usaremos los dos bits de menos peso. La primera, representa la orientación del robot en todo momento, y la segunda, es la variable en la que indicaremos la orientación que queremos tomar, antes de llamar a la subrutina SITUAR. Esta subrutina hará girar al robot hasta que ambas variables coincidan.

ORIGEN, DESTINO y OBJETO, la primera recoge el dato inicial sobre la casilla desde donde parte el robot. OBJETO recoge la posición del objeto, y DESTINO tendrá en todo momento nuestro destino, al comienzo éste será el objeto, y después la salida más cercana.

NODO, tendrá en todo momento el número del nodo en el que el robot se encuentra.

INFO, LIM1A, LIM1B, LIM2A, LIM2B y PADRE están declaradas para recoger los datos en la memoria acerca de cada nodo. Finalmente, AUX1, AUX2, AUX3, y AUX4, sirven para manipular estos datos.

La configuración de entradas/salidas es la siguiente:

OUTPUT 0
OUTPUT 1
OUTPUT 2
OUTPUT 3

Comienzo del programa en sí:

: INICIO

DESTINO = OBJETO

' Debug de la situación inicial

DEBUG "LABERINTO",CR
DEBUG "=========",CR,CR
DEBUG "ORIGEN: ", HEX ORIGEN,CR
DEBUG "DESTINO: ", HEX DESTINO,CR
DEBUG "ORIENTACION: ", BIN ORIENTACION,CR,CR,CR
PAUSE 2000

' El nodo en curso es igual al origen

NODO = ORIGEN

Primero se establece el destino, y tras ello hacemos un debug de la situación inicial del robot. Una pausa de 2 segundos, y actualizamos el valor de nodo, indicando al robot que se encuentra en el origen. Pasamos a la primera lectura sobre el nodo en curso.

LECTURA:

' Lectura del primer byte de info sobre el nodo en curso
READ NODO,INFO
DEBUG "NODO EN CURSO: ", HEX NODO,CR

Leemos el primer byte, que nos indica el número de hijos y la salida más cercana del nodo. Después hacemos un debug indicando el nodo en curso.

: HIJOS

' Evaluamos el numero de hijos que tiene el nodo (INFO.BIT7)

IF INFO.BIT7 = %0 THEN
GOTO UNHIJO
ELSE
GOTO DOSHIJOS
ENDIF

Aquí hemos evaluado el número de hijos que tiene el nodo en curso, y dependiendo del resultado, saltamos a dos etiquetas distintas, ya que en cada caso tenemos que leer un número distinto de bytes de la eeprom.

UNHIJO:

' Leemos 3 bytes ya que tiene un hijo
READ NODO+1,LIM1A
READ NODO+2,LIM1B
READ NODO+3,PADRE

' Depuramos la información sobre los limites de sus descendientes

AUX1 = LIM1A
AUX2 = LIM1B
AUX1.BIT0 = 0
AUX2.BIT0 = 0
AUX1.BIT1 = 0
AUX2.BIT1 = 0

IF AUX1 <= DESTINO AND AUX2 >= DESTINO THEN
SIGUIENTE = LIM1A
ELSE
SIGUIENTE = PADRE
ENDIF

GOTO MOVIMIENTO

Tal como indican los comentarios, leemos la información correspondiente de la eeprom, la filtramos en unas variables auxiliares, y entonces es cuando comprobamos si el destino está entre los descendientes, o avanzando hacia el nodo padre. En el caso de que tenga dos hijos:

DOSHIJOS:

' Leemos 5 bytes ya que tiene dos hijos
READ NODO+1,LIM1A
READ NODO+2,LIM1B
READ NODO+3,LIM2A
READ NODO+4,LIM2B
READ NODO+5,PADRE

' Depuramos la información sobre los limites de sus descendientes

AUX1 = LIM1A
AUX2 = LIM1B
AUX1.BIT0 = 0
AUX2.BIT0 = 0
AUX1.BIT1 = 0
AUX2.BIT1 = 0
AUX3 = LIM2A
AUX4 = LIM2B
AUX3.BIT0 = 0
AUX4.BIT0 = 0
AUX3.BIT1 = 0
AUX4.BIT1 = 0

IF AUX1 <= DESTINO AND AUX2 >= DESTINO THEN
SIGUIENTE = LIM1A
'Si se encuentra entre los limites (el destino es descendiente del nodo (primer hijo))
ELSEIF AUX3 <= DESTINO AND AUX4 >= DESTINO THEN
SIGUIENTE = LIM2A
'Si se encuentra entre los limites (el destino es descendiente del nodo (segundo hijo))
ELSE
SIGUIENTE = PADRE
'Si no se encuentra entre los limites (el destino es ascendente del nodo)
ENDIF

GOTO MOVIMIENTO

Realizamos el mismo proceso, leer, filtrar, y comprobar. En este caso tenemos que tener en cuenta que el destino puede estar entre los descendientes a través de dos hijos distintos, o moviéndonos a través del nodo padre.

MOVIMIENTO:
' Si no se encuentra entre los limites (el destino es ascendente del nodo)

' Capturamos la orientacion
ORI.BIT0 = SIGUIENTE.BIT0
ORI.BIT1 = SIGUIENTE.BIT1
SIGUIENTE.BIT0 = 0
SIGUIENTE.BIT1 = 0

' Si el siguiente nodo al que nos dirigimos es nuestro destino (objeto o salida)
IF (SIGUIENTE = DESTINO) THEN
' Entraremos dos veces en este bucle cuando estemos en el nodo anterior al objeto (elegiremos la salida
' más cercana como destino) y cuando estemos en el nodo anterior a la salida (iniciaremos una serie
' de movimientos que nos llevarán a la salida)
GOSUB OBJETO_SALIDA
ENDIF

' Si hemos elegido la salida $9C, pero el objeto nos bloquea la salida, cambiamos de salida
IF (SIGUIENTE = OBJETO) AND (DESTINO = $9C) THEN
DESTINO = $10
GOTO LECTURA
ENDIF

' Si hemos elegido la salida $10 pero el objeto nos bloquea la salida, cambiamos de salida
IF (SIGUIENTE = OBJETO) AND (DESTINO = $10) THEN
DESTINO = $9C
GOTO LECTURA
ENDIF

' Actualizamos el valor del nodo en curso
NODO = SIGUIENTE

' Llevamos a cabo el movimiento para situarnos sobre el Primero orientamos el robot
GOSUB SITUAR

' Despues avanzamos 20 cm
GOSUB DELANTE

' , una vez realizado el movimiento repite el bucle de lectura de datos el nodo
GOTO LECTURA

El proceso a la hora de realizar el movimiento es el siguiente, primero capturamos la orientación guardándola en ORI. Antes de comenzar a movernos, comprobamos si el nodo al que vamos a avanzar es el destino. La razón, es que si se tratase del objeto, no podríamos avanzar y situarnos sobre él, ya que nos impide el paso. En el caso de que el siguiente nodo sea el objeto o la salida, saltamos a OBJETO_SALIDA.

OBJETO_SALIDA, determinará cual es la salida más cercana, si es que hemos encontrado el objeto, pero no comprobará si este nos bloquea el paso hacia dicha salida. Por lo tanto, en este mismo bloque de código tenemos que hacer esta comprobación y cambiar de salida, en el caso que de que el objeto nos impida el paso.

El proceso al llegar al nodo anterior sería el siguiente:

1. Como estamos en el nodo anterior al destino, saltamos a OBJETO_SALIDA.
2. Esta rutina cambia nuestro destino, estableciendo la salida más cercana.
3. Ahora comprobamos si el objeto bloquea la salida, en caso afirmativo cambiamos de salida
4. Continuamos con la navegación.

El proceso de continuar con la navegación consistiría en, finalmente,
actualizar el nodo en curso, y avanzar sobre él (SITUARNOS Y AVANZAR).

: SITUAR
' Rutina para que el robot se coloque en la orientacion que deseamos, saltando a DERECHA e IZQUIERDA, y realizando los movimientos

' Borramos los bits de más peso inutiles de la orientacion actual (ORIENTACION) y la orientacion que
' deseamos (ORI)
ORIENTACION.BIT3 = 0
ORIENTACION.BIT2 = 0
ORI.BIT3 = 0
ORI.BIT2 = 0

IF ORIENTACION = ORI THEN 'Si hemos alcanzado la orientacion deseada...
RETURN
ELSEIF ORI - ORIENTACION = 1 OR (ORI = $0 AND ORIENTACION = $3) THEN
'Si ORI es una unidad mayor que ORIENTACION
GOTO DERECHA 'o si el robot mira al este y debe avanzar al norte
ELSE
GOTO IZQUIERDA 'En el resto de los casos giramos a la izquierda
ENDIF

Esta subrutina hace que nuestro robot gire para situarse en una orientación que guardaremos en ORI. Básicamente compara los valores de ORI y ORIENTACION, y gira (al hacerlo cambia ORIENTACION) hasta que ambos valores coinciden.

La rutina salta a derecha o izquierda, y se vuelve a repetir, hasta que ambos valores son iguales, y entonces retorna.

DERECHA:
DEBUG "MOV: DERECHA",CR

OUTA = DER
PAUSE PAUSA_DER
OUTA = QUIETO
ORIENTACION =ORIENTACION + 1
GOTO SITUAR

IZQUIERDA:
DEBUG "MOV: IZQUIERDA",CR

OUTA = IZQ
PAUSE PAUSA_IZQ
OUTA = QUIETO
ORIENTACION = ORIENTACION - 1
GOTO SITUAR

DELANTE:
DEBUG "MOV: DELANTE",CR,CR

OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
OUTA = DER
PAUSE PAUSA_CO
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
OUTA = QUIETO
RETURN

Los movimientos DERECHA e IZQUIERDA, son parte de la subrutina SITUAR, hacen un debug notificándolo, y después actualizan la variable que almacena la orientación del robot.

El movimiento de AVANZAR, ejecuta el movimiento, y tras ello retorna.

OBJETO_SALIDA:

' Encaramos el objeto o la salida
GOSUB SITUAR

' Si el destino es la salida $9C estamos en el nodo anterior a esta, y el objeto ya está encontrado, asi que
' iniciamos una secuencia de movimientos que nos sacan del laberinto

IF (DESTINO = $9C) THEN
DEBUG "MOV: DELANTE",CR
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
OUTA = DER
PAUSE PAUSA_CO
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
OUTA = IZQ
PAUSE PAUSA_IZQ
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR
DEBUG "MOV: IZQUIERDA", CR
DEBUG "MOV: DELANTE",CR
GOTO FIN
ENDIF

' Si el destino es la salida $10, estamos en el nodo anterior a esta, y el objeto ya está encontrado, asi que
' iniciamos una secuencia de movimientos que nos sacan del laberinto

IF (DESTINO = $10) THEN
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
OUTA = DER
PAUSE PAUSA_CO
OUTA = AVANZAR
PAUSE PAUSA_AVANZAR/2
DEBUG "DELANTE",CR
GOTO FIN
ENDIF

' Si llegamos aqui, significa que el destino es el objeto, y acabamos de encontrarlo, estamos en el nodo
' anterior y lo hemos encarado, asi que ahora actualizamos el destino con la salida mas cercana

IF INFO.BIT0 = 0 THEN
DESTINO = $10
ELSE
DESTINO = $9C
ENDIF

' Repetimos todo el proceso
RETURN

Esta rutina ha de ser llamada cuando nos encontramos en el nodo anterior al destino, que puede ser el objeto o la salida. Su función es, salir si se trata de la salida, o determinar la salida más cercana si hemos encontrado el objeto.

Para ello, primero llama a SITUAR, es decir, si el destino es el objeto, encara el objeto, y si se trata de la salida, simplemente se prepara para avanzar.

Después tenemos dos bloques que evalúan si el destino es alguna de las salidas, y en tal caso, ejecutan una serie de movimientos que nos llevan hasta la salida. Para cada salida hay un bloque distinto de movimientos. Ambos acaban saltando a la etiqueta FIN, que finaliza el programa, avisando mediante un debug.

Si no se ejecuta ninguno de estos dos bloques, significa que el destino es el objeto, y entonces, teniendo en cuenta el nodo en el que nos encontramos, (nodo anterior al objeto), especifica la salida más cercana mediante el bit correspondiente en sus datos. Tras ello, retorna.

FIN:
DEBUG CR,CR,"FIN DEL PROGRAMA"
END

Una vez salimos del laberinto, el programa lo indica mediante un debug, y acaba.


Para descargar el archivo comprimido del código del programa (2,43 KB) pulsar el siguiente enlace:
http://www.todomicrostamp.com/data/programalaberinto.zip


ACERCA DE LA ESTRUCTURA FISICA

No me voy a extender sobre la estructura física, ni sobre detalles de electrónica, por que no es el punto fuerte de mi proyecto. Considero que es la parte mejorable, la parte que no tuve tiempo ni paciencia para desarrollar mejor, e invito a cualquier lector a tomar lo que he empezado y continuarlo, mejorarlo, tanto a nivel de software como a nivel de hardware.

Para construir el robot he usado un microcontrolador Basic Stamp II, en la placa de un HomeBoeBOT. Para el movimiento, servomotores trucados para funcionar como motores de CC, manejados por un driver de motores basado en el circuito L293B, y alimentados con 6V (4 Pilas de 1,5 V). La placa en sí está alimentada con una pila de 9V. Para introducir la coordenadas, me decidí por utilizar un mando de televisión bajo el standard de Sony, y un receptor de I.R. incluido en el pack del HomeBoeBOT, para recibir la señal de este mando, y aceptar secuencialmente nos números correspondientes a las coordenadas de salida y del objeto, así como la orientación inicial. Además de estos, no tiene ningún sensor para detectar las paredes.

La estructura para soportar el robot, está basada en placas finas de metacrilato, unidas por barra roscada, tornillos y tuercas.


CONCLUSION Y MEJORAS

Con esta documentación, espero haber conseguido mi objetivo, compartir de una manera sencilla el trabajo que he realizado sobre este laberinto, que considero que es aplicable a otros laberintos distintos. En cuanto a la falta de información acerca de su estructura, motores, etc, como ya he explicado, se debe a su sencillez, y su falta de calidad en comparación con el algoritmo. He de decir, que el robot consigue salir del laberinto y realizar su tarea, con pequeñas correcciones manuales en su trayectoria, debido a la imprecisión de los movimientos. Una posible ampliación, sería añadirle algún sistema de corrección de la trayectoria, en función de diversos sensores medidores de distancias, como los ultrasónicos, brújula electrónica, retroalimentación basada en encoders, etc.

Proyecto en archivo con formato PDF (1,2MB):
http://webs.ono.com/lmoliver/proy_xabier_laberinto.pdf