Robotica educativa con Arduino y BASIC Stamp
Robot de laberinto [Xabier Ugarte Pedrero]
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 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.
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. |
 |
|||
También hemos trazado el camino más corto entre los dos puntos. He aquí el potencial de esta numeración.
Para representar la orientación en 2 bits, utilizamos en siguiente esquema. FORMATO DE LA INFORMACION DE CADA NODO
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. El mapa del laberinto, si cambiamos los números de nodo, y volvemos a dotar al árbol con su forma |
|||
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). DATA @$00, $00, $04, $AC, $00 NUESTRO ALGORITMO |
|||
 |
|||
|
Etiquetas: infoavances, laberinto
Respuestas a esta discusión
-
webmasterPermalink Responder para Lorenzo M. Oliver el -
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 -
© 2012 Creado por Lorenzo M. Oliver.
[Imágenes licenciadas por Depositphotos.com] Tecnología de
