Aplicación para el patito

La anterior entrada hacía referencia a una interfaz gráfica para ver de forma mas bonita el robot que hará de guía. Ahora le ha tocado el turno al otro robot. Esta aplicación es muy similar a la anterior, con mínimos cambios:

• Se ha eliminado la velocidad del robot, pues esta ya no es controlable, será el propio robot quien establezca la velocidad a la que debe moverse.

• Se ha incluido una imagen que representa un rectángulo con los cuatro puntos 'vistos' por la cámara de infrarrojos del wiimote. Nos da una idea de que está viendo el robot a tiempo real. Él se basará en estos puntos para determinar la distancia con su 'mamapato' y la dirección a tomar.

• Como tonterías añadidas, se puede cambiar los colores y la resolución del visor

Otra aplicación mas

Después de varias intentonas, al fin he conseguido una aplicación gráfica útil para mis propósitos. En esta ocasión es un interfaz gráfico para controlar el robot de cabeza, o "Mama Pato". Una vez han sido conectado Wiimote y Brick por medio de la cruceta del mando puede controlarse el robot como si de un coche teledirigido se tratase.

No tiene nada especial, activar algunas 'pijotadas' para el mando como leds o vibración, controlar la velocidad de movimiento del robot...

Desde Barna con amor

Hace tiempo que no pongo un vídeo de robots chorras, así que aquí va. ¿Qué objetivo puede haber en la vida mejor que el de buscar objetos para hacer música?



El vídeo me lo ha enviado Pepe, que "es una gran persona y mejor ser humano". ¡Gracias amigo!

La distancia entre dos puntos

Pasamos a sacarle partido a la cámara de infrarrojos del wiimote. Para empezar vamos a calcular la distancia (en pixeles) entre dos puntos, mas adelante usaremos esta información para saber lo lejos (o cerca) que nos encontramos de un objeto que disponga de dos emisores infrarrojos que están separados el uno del otro por una distancia conocida.

El primer paso, entonces, se trata en conocer la distancia entre estos dos puntos. Para verlo todo de formas mas gráfica he utilizado la extensión pygame y he tomado unas 'fotos' para contar los pasos seguidos.

Partimos de un momento 't', en el que leemos dos puntos en la cámara del mando, estos dos puntos, como indica la figura 1, son A y B

figura 1

De cada punto conocemos sus coordenadas, lo cual nos facilita enormemente el trabajo. A estas coordenadas las llamaremos (Xa, Ya) (Xb, Yb). Como los ejes X y Y son perpendiculares, trazamos dichos ejes atravesando nuestros puntos, obteniendo el resultado de la figura 2

figura 2

A la intersección de ambos ejes la denotaremos como C, con un poco de imaginación puede verse que la recta AB es la hipotenusa formada por los vértices ABC (figura 3).

figura 3

Recordando uno de los teoremas de Pitágoras sabemos que h2 = c2 + C2. (el dos indica cuadrado, a falta de Latex...)

Sólo nos faltaría saber las coordenadas de C, o bien la longitud de los lados AC y BC, algo que tenemos que extraer de la información disponible. Para conocer la distancia AC basta con restar la coordenada Xb a Xa, o siendo mas precisos, el valor absoluto de esta diferencia.

BC lo obtendremos por el mismo procedimiento, y será igual al valor absoluto de la diferencia de Ya e Yb

Ya disponemos de la distancia, en píxeles, de ambos puntos. El siguiente paso es utilizar esta información para saber la distancia a la que nos encontramos del objeto que tiene los emisores

NOTA: Me habría encantado utilizar Latex para las fórmulas, pero la solución que me pasó Javi parece no funcionar en linux

El tiro por la culata

Al cabo de un rato terminé encontrando el motivo por el que recibía esos valores negativos tan extraños. Aunque todo indicaba lo contrario el valor devuelto por la instrucción xhat[-1] es correcto para el vector xhat (que raro que yo me equivoque).

Al principio pensé que el problema radicaba en que yo solicitaba el valor del vector cuando este estaba siendo tratado por el filtro. Me explico: actualmente tengo dos clases, una llamada Kalman encargada de filtrar la información cuando se agreguen valores leídos por el mando, y la clase Wiimote que entre otras muchas cosas trata las "interrupciones" del mando, entregándosela al objeto kalman para que la filtre, un momento después se solicita el último dato del vector ya filtrado para así calcular velocidad y espacio en tiempo real.

Para evitar esto (un error de programación por mi parte) creé un vector nuevo, copia del anterior, que sólo se actualiza cuando toda la información ha sido filtrada, de forma que siempre devolverá un valor filtrado (aunque sea uno que ya ha sido entregado). Pero esto tampoco funciona, los valores extraños siguen apareciendo, y ahora mismo no se como resolverlo.

Calcularé velocidad y espacio una vez terminado un movimiento, no es igual de 'bonito' pero creo que es la última baza que me queda para utilizar el mando (al menos con el enfoque actual)

El que la sigue la persigue

Ayer por fin encontré donde está el fallo, el por qué no funciona, la línea que estropea todo... pero lo mas triste es que no se como repararlo.

El problema está, y no está, en el filtro Kalman. La implementación que tengo de él funciona perfectamente. Este algoritmo crea un array perteneciente al paquete numpy (que proporciona arrays mucho mas útiles que los de python estandar) para almacenar los datos filtrados. Es de este array (al que llamo xhat) del que extraigo los datos para calcular la velocidad y el espacio.

Ahora un breve repaso al manejo de arrays en python

Creamos una variable 'vector' que contiene tres números, en este caso enteros

>>> vector = numpy.array([1, 2, 3])

Si queremos acceder a la primera posción (contando desde 0) sólo tenemos que ejecutar

>>> vector[0]
1

Para un vector de números 'flotantes'

>>> vector = numpy.array([1.1, 2.1, 3.1])
>>> vector[-1]
3.1

Con el índice negativo comenzamos a contar desde el final

Por defecto, el paquete numpy crea arrays de tipo float64 que permite almacenar números mas grandes, aunque esto es algo que puede modificarse en el momento de crear el array.

El problema viene ahora, el algoritmo rellena el array xhat, de tal forma que si quisiera obtener el último elemento de este para hacer mis cálculos, ejecutaría una instrucción tan sencilla como esta

>>> xhat[-1]

y al menos yo esperaba obtener un número 'flotante', pero... no. Por algo que no entiendo, que parece ser contra natura, obtengo otro número. Si, otro número. No es el número xhat[-1] redondeado, no. Ni siquiera parecido, es, simplemente, otro número. Que en la mayoría de los casos viene a ser algo aproximado de -1.04... que provoca que la velocidad descienda por debajo del 0

¿Por qué?

Vaya usted a saber, yo llevo desde ayer preguntando a google y no me dice nada coherente. Lo bueno es que he encontrado el problema, lo malo, que roza lo absurdo.

Menos mal que al ser tan cabezota terminaré encontrando la solución, y seguro que es una tontería

Pruebas, pruebas, pruebas...

Desde la última entrada he realizado pruebas con el código actual. Los resultados, para ser francos, no son muy esperanzadores. En ocasiones la medición es de una exactitud asombrosa, y en otras, son un desastre. No me refiero a que el error cometido sea de 5 cm, ni siquiera 10. Muchos experimentos arrojan medidas negativas de dos decenas de centímetros, y eso no es muy normal.

He revisado ya varias veces el ćodigo referente a la cinemática, lo he contrastado con libros de física y parece ser todo correcto. El filtro de kalman parece funcionar correctamente, ya que la aproximación gráfica que obtengo de la aceleración se ajusta increíblemente bien a la realidad. Y sin embargo, en muchos experimentos termino obteniendo una velocidad negativa y por consiguiente un retroceso en el espacio. Aún dejando varios segundos el mando en reposo, sin que exista mas fuerza que la gravedad actuando sobre él (fuerza que para mi experimento no es significativa)

He encontrado un artículo bastante técnico sobre este asunto (aplicado también al wiimote) que he releído varias veces. Es posible que mi tosco inglés no me permita sacar todo el jugo al texto, pero creo que lo esencial si lo aplico en mi algoritmo.

De momento, sigo con pruebas, pruebas y pruebas...