Perdido pendrive

Este post es un poco raro.
He perdido el pendrive, y en él estaba escrito mi nombre. Si alguien lo ha encontrado, puede que llegue hasta aquí.

Le pido por favor que se ponga en contacto conmigo en la dirección alexlad_87@hotmail.com

Muchas gracias.

Prueba final en Simulador JDE/Gazebo Alex

He sacado un vídeo de la prueba con el Departamental lleno de papeleras.

Existen dos papeleras en las que el robot falla al llegar a ellas, sin embargo, si quitamos ese factor (las papeleras separadas, corrigiendo la orientación teleoperando) el robot realiza el resto del recorrido, aunque con algún problema a la hora de dar la vuelta (aunque finalmente lo consigue).

En este vídeo podemos verlo. Se choca con un par de ellas a la vuelta, pero eso es porque al estar grabando la pantalla no funciona como debería. Lo he probado varias veces sin estar grabando y sí funciona.




Sin embargo me he dado cuenta de un error que tenía:
A la hora de decidir si seguía las indicaciones de seguridad, tenía esto:
if (segD > 10 || segI > 10)

Sin embargo, me he dado cuenta de que no es lo que necesitaba, es esto:
if (abs(segD - segI) > 10)

He vuelto a probarlo y ahora consigue pasar bien, hace el recorrido perfectamente.




Mueve un poco la pared, pero no lo hace si no estoy grabando. Se acerca mucho, pero no llega a tocarla.

Simulador JDE/Gazebo Sara

Hemos dejado nuestro robot físico, para adentrarnos en el mundo de los robots simulados, con el entorno Gazebo. Disponíamos de un mundo con el diseño del departamental y nuestro robot tenía que navegar por él sin chocarse, ayudados de las imágenes que obtuviéramos por la cámara, nuestro único sensor.
Una vez definido el problema, había que pasar a solucionarlo… Lo primero que hice fue pasarle un filtro de color, para distinguir el suelo del resto del entorno que es lo que le estorbará al robot para navegar. Una vez hecho esto, pensé…bueno y ahora que?? Lo primero que se me ocurrió fue ir comprobando (de abajo hacía arriba, y de izquierda a derecha) los pixeles que pertenecían al suelo, y poner un umbral de pixeles mínimos de suelo que debía haber, y si encontraba ese umbral que el robot se parase (girase…)… Dándole unas vueltas a esto, me di cuenta de que no iba a llegar muy lejos… tenía un problema grande, que pasaba si me encontraba un objeto en medio del pasillo?? Era muy probable que mi robot se estampara con él, porque seguramente la suma de los pixeles a la derecha e izquierda del objeto superasen ese umbral.
Desestimada esta idea, busqué una solución más fácil, cogería una fila de la imagen y la analizaría. Elegí la fila 210, estaba por debajo de la mitad y parecía una distancia razonable para detectar objetos con los que poder chocar (para no hacerlo claro). Y lo que hacía a continuación era bastante sencillo. Simplemente buscaba el hueco más grande de píxeles “pasillo” que hubiese en el array, calculaba el centro y hacía girar al robot mas o menos en función de ese valor medio, es decir, si me daba mas o menos alrededor del centro, el giro iba a ser cero, si me daba un poco por encima (o por debajo) le haría girar un poco y si me daba muy por encima (o muy por debajo) el giro sería mas brusco. Pues bien, esta solución tan sencilla funcionó a la primera, recorría perfectamente el pasillo y con la idea de ir siempre al centro del grupo de píxeles “pasillo” más grande, se centraba solo el robot, por lo que navegaba por el centro del pasillo sin riesgo a rozar alguna pared y que se viniera abajo todo el departamental.
El problema vino cuando nos cambiaron el mundo…y nos añadieron miles de papeleras… en un principio yo pensé bueno, que mas da una que veinte?? Si el robot pasa una bien, tiene que pasar las veinte bien… pero ahí teníamos la “maldita papelera” en una esquina, cosa imprevista que mi robot al hacer un giro tan brusco a la derecha se comía de lleno. Así que le bajé un poco la velocidad angular y le aumenté la velocidad (sí, antes iba aún mas despacio), probé pero seguía comiéndose la papelera, total que había que bajar aún mas esa velocidad de giro, pero ya no me dio tiempo a cambiarla, porque la competición empezaba, así que mi robot se quedó en la papelera del infierno.

Aquí os dejo el video para que podais verlo.



Y mi código.

void yourcode_iteration(void)
{
int y, x, i, j;
int r,g,b;
char matriz[SIFNTSC_COLUMNS][SIFNTSC_ROWS];
int mediorow, mediocol;
int maxina = 0;
int maxact = 0;
int beginact = -1;
int beginina = -1;
int comienzaact=-1;
int comienzaina = -1;
int seguidosact = 0;
int seguidosina = 0;
int medio;
int entra = 0;
int entra2 = 0;
v = 350;
for (y = 0; y < SIFNTSC_ROWS; y++)
for (x = 0; x < SIFNTSC_COLUMNS; x++) {
b = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3];
g = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1];
r = (int)(unsigned char)imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2];
if (((r > 230) && (r <= 255)) && ((g > 230) && (g <= 255)) && ((b > 110) (b < 140))) {
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3] = (unsigned char)255;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1] = (unsigned char)255;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2] = (unsigned char)255;
matriz[x][y] = '.';
}
else {
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3] = (unsigned char)0;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+1] = (unsigned char)0;
imagenRGB[(y*SIFNTSC_COLUMNS+x)*3+2] = (unsigned char)0;
matriz[x][y] = '_';
}
}
j = 210;
for (i = 0; i < SIFNTSC_COLUMNS; i++) {
if (matriz[i][j] == '.') {
if (maxina < seguidosina) {
comienzaina = beginina;
maxina = seguidosina;
}
beginina = -1;
seguidosina = 0;
if (beginact == -1) {
beginact = i;
seguidosact = seguidosact +1;
}
else
seguidosact = seguidosact +1;
}
else {
if (maxact < seguidosact) {
comienzaact = beginact;
maxact = seguidosact;
}
beginact = -1;
seguidosact = 0;
if (beginina == -1) {
beginina = i;
seguidosina = seguidosina +1;
}
else
seguidosina = seguidosina +1;
}
printf("posicion: [%d,%d], %c\n", i,j, matriz[i][j]);
}
if (maxact < seguidosact) {
comienzaact = beginact;
maxact = seguidosact;
}
else if (maxina < seguidosina) {
comienzaina = beginina;
maxina = seguidosina;
}
printf("maxact %d", maxact);
printf("begin %d", comienzaact);
medio = ((comienzaact + maxact) + comienzaact)/2;
printf("medio %d", medio);
if ( medio >= 0 && medio <= 75){
w = -65;
v=0;
}
else if(medio > 75 && medio <= 125) {
w = -30;
v=0;
}
else if (medio > 125 && medio <= 140) {
w = -10;
v=0;
}
else if (medio > 140 && medio <= 180) {
w = 0;
}
else if (medio > 180 && medio <= 195) {
w = 10;
v=0;
}
else if (medio > 295 && medio <= 245) {
w = 30;
v=0;
}
else {//if (medio > 235 && medio <= 259)
w = 65;
v=0;
}
//_exit(" ");
printf ("salir");

}

Simulador JDE/Gazebo Alex

En esta parte dejamos a Bender, que ya está un poco mayor y trabajamos con el simulador JDE y Gazebo, que proporciona varias herramientas de un robot virtual pioneer, como laser, y cámara en un mundo modelado, en este caso del departamental de la universidad.
Las prácticas se programan en este caso en C, en lugar de Java, como estabamos acostumbrados hasta ahora.
Se trata de deambular por este mundo utilizando tan sólo la visión de la cámara. Para ello nos aconsejaron aplicar un filtro a la imágen (que tenemos en una variable como array de char). Por ello lo primero es crear un filtro de color que detecte el amarillo del suelo (255,255,126 en RGB).
En principio es simple, el problema aquí fue un poco los casting necesarios para pasar de char a entero. Básicamente pasamos de char a unsigned char, y de ahí a int. Al contrario, sólo pasamos a unsigned char.

Ésta es la función.
void filtro (int cr, int cg, int cb, int threshold)
{
int x,y;
int r,g,b,pos;

for (y=0; y <>
for (x=0; x
{
pos = (y*SIFNTSC_COLUMNS + x)*3;
b = (int) (unsigned char) imagenRGB [pos];
g = (int) (unsigned char) imagenRGB [pos+1];
r = (int) (unsigned char) imagenRGB [pos+2];
if ((r > (cr-threshold) && r < (cr+threshold) && g > (cg-threshold) && g < (cg+threshold) && b < (cb+threshold) && b > (cb-threshold)))
{
imagenRGB [pos]= (unsigned char) 255;
imagenRGB [pos+1]= (unsigned char) 255;
imagenRGB [pos+2]= (unsigned char) 255;
}
else
{
imagenRGB [pos]= (unsigned char) 0;
imagenRGB [pos+1]= (unsigned char) 0;
imagenRGB [pos+2]= (unsigned char) 0;
}
}
}

Threshold es utilizado para indicar el rango que aceptamos por bueno. Por ejemplo, en este caso para detectar el amarillo, escribía:
filtro (255,255,126,10);
Por tanto, si era 246,255,135 detecta el color como amarillo.

Los resultados se modifican en la misma variable global, escribiendo blanco (255,255,255) si encuentra amarillo, y negro (0,0,0) en caso contrario.

Para comprobar que era correcto me hice un "visualizador" chapucero, escribiendo espacios para no negro y puntos para negro (aunque realmente sólo miro un color, después del filtro todos son iguales).
Como obviamente no cabe todo en la pantalla (la resolución de la imágen era de 320x240), tomaba muestras para que encajara en 80x50:

for (y = 0; y < x="0;" relx =" x*(SIFNTSC_COLUMNS/80);" rely =" y*(SIFNTSC_ROWS/50);">
A la hora de analizar la imágen, me decidí por parametrizarla, sacar ciertos valores que fuesen determinantes. Estos fueron los que tomé:


Mi intención era era utilizar los ángulos de inclinación izquierdo y derecho (alpha y beta). Pero esto incluía multitud de problemas no sabía si era muy fiable, y al final no lo incluí.
Este es el código:

void analizarImagen (int *h, int *alpha, int *beta, int *latX, int *latY, int *j, int *k, int *segI, int *segD)
{
int x,y;
int maxY=0;
int maxX=0;
int seguidos=0;

//Hallamos el punto más alto
for (x=0; x 0 && ((int) (unsigned char) imagenRGB[(y*SIFNTSC_COLUMNS + x)*3]) == 255)
y--;
if (x==SIFNTSC_ROWS/2)
*h=SIFNTSC_ROWS-y;
if (SIFNTSC_ROWS-y > maxY)
{
maxY=SIFNTSC_ROWS-y;
maxX=x;
seguidos=0;
}
else if (SIFNTSC_ROWS-y == maxY)
seguidos++;
}

//Hallamos medida de seguridad izquierda
y=SIFNTSC_ROWS-1;
x=0;
while (x < segi="x;" y="SIFNTSC_ROWS-1;" x="SIFNTSC_COLUMNS-1;"> 0 && ((int) (unsigned char) imagenRGB[(y*SIFNTSC_COLUMNS + x)*3]) != 255)
x--;
*segD=SIFNTSC_COLUMNS-x;

//Hallamos desviación izquierda
y=SIFNTSC_ROWS-maxY+1;
x=0;
while (x < latx="x;" y="SIFNTSC_ROWS-maxY+1;" x="SIFNTSC_COLUMNS-1;"> 0 && ((int) (unsigned char) imagenRGB[(y*SIFNTSC_COLUMNS + x)*3]) != 255)
x--;
*latY=SIFNTSC_COLUMNS-x;

//Hallamos altura izquierda
x=0;
y=SIFNTSC_ROWS-1;
while (y > 0 && ((int) (unsigned char) imagenRGB[(y*SIFNTSC_COLUMNS + x)*3]) == 255)
y--;
*j=SIFNTSC_ROWS-y;

//Hallamos altura derecha
x=SIFNTSC_COLUMNS-1;
y=SIFNTSC_ROWS-1;
while (y > 0 && ((int) (unsigned char) imagenRGB[(y*SIFNTSC_COLUMNS + x)*3]) == 255)
y--;
*k=SIFNTSC_ROWS-y;
}

En un principio la altura h era el punto máximo, después vi que está idea no era buena y decidí que fuese la medida del punto medio.
Quizá esto fue uno de mis errores, era demasiado variable y tenía altas probabilidades de error y no ser lo que quería, quizá hubiese sido mejor una media de varios puntos por el centro.
Las medidas segI y segD las incluí después y no estaban previstas, y sin embargo han resultado bastante útiles.
Después de esto ya sólo quedaba la lógica del programa, es decir, como utilizar estos datos.
Había considerado varios casos, y luego añadí algunos más.
En la imágen de arriba vemos el caso simple, sólo hay que avanzar.
Lo primero que hacíamos era atender a las variables segI y segD, que pueden indicarnos un choque inminente, lo que hacemos es que la velocidad angular sea proporcional a la diferencia de ambas. De esta fomra girará más o menos rápido según la necesidad.
Esto sirve para tres cosas:

1. Choques inminentes
2. Centrarse en el pasillo
3. Ayuda en los giros, cuando lleguemos aproximadamente a la mitad del giro una será mucho más grande que la otra, y girará de forma adecuada.

Me paso con casi todas las variables y lo comentaré ahora.
Esta lógica de proporcionalidad parece estar bien, pero como vimos en clase y comprobé en la práctica, tiene grandes fallos. Con una proporción simple el robot oscila muchísimo. Implentar un control PID se me hacía algo complicado, porque no tenía una función de como van cambiando las variables. Lo que hicé para evitarlo fue hacer una media entre la diferencia de las variables, y la velocidad angular anterior: (segD-segI +w )/2.
Aquí tuve que probar varias cosas, como aumentar la proporción que dependía de la medida anterior para que acelerara más despacio, por ejemplo (segD-segI + w*2)/3.
En la realidad este equilibrio era un poco precario.

También tengo que decir que en los giros tube muchísimos problemas, porque en principio no me funcionaba bien, todo ello se debió a que se me olvidó aplicar la media a estas dos variables y se chocaba con la pared interior por eso.

Si las dos anteriores eran muy parecidas, utilizaba j y k.
Realmente son muy parecidas a las anteriores, pero las sustituye cuando estamos en una situación como la de la imágen superior, en pasillos más estrechos.
Si también son muy parecidas, quiere decir que estamos centrados y andamos rectos.
Entonces utilizamos las variables latX y latY, que nos conducen al centro del pasillo, y si hay obstáculos que hacen que el centro esté desplazado, tienden a este.
Aquí también pudo haber un pequeño problema, ya que sólo se tiene en cuenta si las anteriores son las correctas. Quizá debía haberlas incorporado antes.

Esto debía ser suficiente para resolver cualquier situación, pero en los giros w se hacía demasiado alta y chocaba con la pared anterior.
Después me di cuenta de que se debía al problema de la media que dije antes, que no me di cuenta de hacerla para segI y segD.
Por eso utilicé un pequeño truco, acercarme a la pared.
En principio, el giro debía comenzar en el momento en que desaparece la pared del lado al que vamos a girar. Lo que hicé fue que en ese momento actuara otro código, que ponía w a cero, y sólo actualizaba v (no lo he dicho antes, pero v es proporcional a la altura h, de forma suave, con la media, como en el resto de medidas y además en ocasiones le restamos w, para que no vaya muy rápido al girar). Este código dejaba de ejecutarse si h era menor que una cierta medida, que estaba puesta entre 40 y 55).
En ese momento se dejaba actuar el código normal.
Como aún así, no solucionaba correctamente el problema (por lo dicho antes), pasé a crear otro fragmento de código especial para el giro.
Al posicionar, poníamos una variable (girando) a 1, de forma que al dejar de ejecutarse, entrara en la parte del código destinada a girar. Giraba de una forma más suave, y aún no estaba terminada cuando me di cuenta del error de la media.
Entonces rápidamente abandoné esta parte, y comprobé que funcionaba correctamente.

Ahora me encontraba con otro problema, al llegar a la parte donde tenemos dos posibles giros, no sabía que hacer, porque al ir justo por el centro y acercarse, no se decantaba por ningún lado, o giraba bruscamente. Entonces retomé el código de girar, pero sólo entraba cuando teníamos esta situación, entonces simplemente si no había ninguna posibilidad de decantarse, por defecto w pasaba a valer -3, lo suficiente para que se decantara por la derecha.
El código de girar deja de ejecutarse cuando estamos más o menos encaminados.
(La altura es mayor que la diferencia de los lados, y además es mayor de 30).

No había tenido en cuenta hasta ahora la posibilidad de tener que dar la vuelta. Así que al final cree un código que si detectaba poca velocidad (que implica poca altura) y que la parte superior de la imágen es estrecha, gira a un lado u otro en función de donde esté situada la parte superior.

También incluí después un limitador de velocidad angular (si sobrepasaba 40, en un sentido u otro, pasaba a ser 40).

Este es el código de esta parte:
analizarImagen (&h,&alpha,&beta,&latX,&latY,&j,&k,&segI,&segD);

if (((abs (h-k) <>55) //Va a girar, se acerca un poco
{
w=0;

v=h*10;
printf ("Posicionando");
if ((abs (h-k) <>
giro = 0;
else
giro = 1;
}
else
{
hInicial = -1;
if (giro == 0) //Está girando&& (abs (j-k) > 10)
{
v = h*10;
if (j > k)
{
ladoRelevante= k;
w = 30;
}
else
{
ladoRelevante= j;
w = -30;
}

if (h > (2*abs (j-k)))
{
//Giros lentos, porque está terminando
w = (latY-latX)/4;
if (w == 0)
w = (j-k)/4;
if (w == 0) //Si no sabe donde girar, a la derecha
w = -3;
}


//Si ya hemos encotrado el final del pasillo y más o menos estamos posicionados
//dejamos de girar (la altura máxima es mayor que la diferencia de los lados)
if (h > abs (j-k) && (ladoRelevante <> 30)
giro=1;
printf ("Girando");
}
else
{
giro = 1;

if (segD > 10 || segI > 10)
w = (segD - segI + w*2)/6;
else
if (abs (j-k)<1)
if (abs(latX-latY)<1>
w=0;
else
w = ((latY-latX) + (j-k))/2; //Media con lo que daría el otro para que no oscile
else
w = ((j-k)+w*3)/6;

v = (h*20 + v*2)/4 - abs(w); //Para que no oscile
}
}
if (v <>1 && segD>1)
{
v=0;
if (latX > latY)
w = -20;
else
w = -20;
}

if (w>40)
w=40;

if (w<-40)
w=-40;

Finalmente conseguí que el robot fuera más o menos deprisa (unos 1000 en recta, 1200 incluso), y recorriera cualquier parte del mundo que teníamos, girando a izquierda, derecha, y dando la vuelta. En el vídeo podemos verlo. Va unas 5 veces más rápido que la realidad, porque el capturador de la pantalla quería coger imágenes a 10 fps y sólo lo consiguió a 2 fps.



En la práctica final sólo llegué a la tercera papelera. El problema era que había demasiado hueco detrás de algunas papeleras, y esto no estaba previsto. Con los ángulos quizás hubiera ido mejor, o incluyendo esa posibilidad, o tomando las medidas de una forma distinta.
Si no hubiese sido por esto, habría ido rápido y bien, y el único problema quizá hubiera sido el dar la vuelta, porque estaba abierto.
El giro de la última esquina, con esa papelera tan "bien" puesta, lo probé después y lo pasaba bien, sin llegar a tocarla (aunque por poco).

Visualización en pantalla

Y llegaron las comunicaciones.
Desde hace un tiempo, Bender conseguía comunicarse con el ordenador y traspasaba todos los datos que recogía, así como los que calculaba.
En el robot era muy sencillo implementar la interfaz gráfica. Con la clase LCD no teníamos más que indicar los dos puntos de una recta, y ésta se imprimía en la pantalla, sin tocar nada más.
Con el ordenador la cosa se complica. Es necesario utilizar alguna de las clases que proporciona Java, y éstas no son muy intuitivas.
Pero finalmente conseguimos imprimirlo, creamos la ventana, transmitimos los datos, y dibujamos con el método paint.
Lo que hicimos fue heredar de la clase JFrame, que es algo parecido a una ventana.
Después no tenemos más que sustituir el método paint, e incluir en main una secuencia de instrucciones que crean el nuevo JFrame y lo ejecutan en un Thread aparte.
Aquí surgió un error, ya que para recibir los datos, lo hacíamos en main, justo después de crear el nuevo Thread, y la variable global donde poníamos los datos no quedaba actualizada y no imprimía nada.
Lo que hicimos puede considerarse una pequeña chapucilla, aunque creo que no lo es.
En el método run del nuevo Thread, incluímos todo el código destinado a la comunicación con Bender. Así conseguimos que funcionara.
Antes de esto, como pensaba que podían funcionar en threads distintos, incluí bloque synchronized antes de modificar o leer la variable global donde estaban los datos.

Sin embargo surgió otra complicación.
El siguiente paso obvio era no tener que lanzar el programa cada vez que queremos imprimir un escaneo, no tiene sentido entonces crear una nueva conexión cada vez. Así que tubimos que modificar el programa de Bender, para que sólo crease un nuevo canal de comunicación si no tenía ninguno antes.
Sin embargo, al lanzar el prama, no se ejecutaba el método paint, o sólo lo hacía una vez al principio.
Esto se debía a que pusimos un bucle infinito en el código de las comunicaciones, para que siempre esté pidiendo nuevos datos. Y el método para repintar la ventana no se ejecuta automáticamente.
En principio pensaba que podía tratarse de que debían ir en Threads distintos (y de hecho sería mejor así) y probé haciendo yield pero no funcionó. Después me di cuenta que en el programa anterior sólo se actualizaba una vez.
Así que utilizamos un método de la ventana, repaint, pero no funcionaba.
Por fin, un poco por probara, ejecutamos el método paint. Él problema de éste método es que hay que pasarle un objeto Graphics, entonces probé con un método que tiene ventana, getGraphics, y por fin funcionó bien.

Aquí podemos ver dos vídeos, el primero está configurado con una resolución de un grado.



El segundo con cinco grados.



¡Y aquí lo podemos ver en acción!



Para las conexiones con el ordenador utilizamos el USB. Está todo preparado para, modificando una variable (blue=true) utilicemos la conexión por Bluetooth, sin embargo no he conseguido que funcione el driver, y por tanto no lo puedo mostrar.

El radar: Última actualización

Finalmente conseguimos implementar la función para los pasillos.

Intentaremos explicarlo brevemente.



El objetivo es no chocarse con nada.
En principio, si en ese rango de 20º (por defecto, aunque puede variar) no tenemos ningún obstáculo, suponíamos que podíamos pasar. Sin embargo, si en ese pasillo, del mismo ancho que el robot y centrado en la dirección que evaluamos, existe algún punto que queda fuera del ángulo, vamos a chocarnos con él.
Esto es algo que puede ocurrir, por ejemplo, cuando intentamos pasar por una "puerta" y estamos demasiado cerca.
Lo que tratamos de hacer ahora es comprobar si tenemos algún punto de estos, y en caso de encontrarlo, éste será el que defina la nota para esa dirección.

Lo que necesitamos es conocer la distancia desde la recta de la dirección candidata al cada uno de los puntos que hemos tomado con el sensor.
En el caso esa distancia (d) sea menos que la mitad del ancho del robot, significa que está dentro del pasillo. Lo que hacemos entonces es calcular la distancia que puede recorrer el robot, que es la distancia en la misma dirección theta desde la recta perpendicular al punto (min).
Por tanto exploramos para cada dirección theta, todos los puntos que tenemos y nos quedamos con el mínimo de todos los que entren en el pasillo.
Realmente no probamos todos los puntos, sino los que varían como mucho 90º de la dirección theta (el resto no influyen) y por supuesto de aquellos de los que tenemos datos.

La función implementada en java es esta:
private static int pasillo (int dir, int[] resultado)
{
minim = 255;
//No interesa lo que queda fuera de la recta perpendicular
for (i = Math.max(0, (dir-90)/resol); i <>
{
//Calculamos la distancia entre el punto y la recta de la dirección
dis =(int)(Math.abs((Math.tan(Math.toRadians(dir)))
*Math.cos(Math.toRadians(i*resol))
*resultado[i]
-Math.sin(Math.toRadians(i*resol))*resultado[i])/
(Math.sqrt(Math.pow((Math.tan(Math.toRadians(dir))),2)+1)));
//Comprobamos si entra en el pasillo
if (dis < (anchoRobot/2))
{
//En caso de entrar comprobamos cuanto podemos avanzar sin chocar
if ((i*resol) <>
{
if (Math.sin(Math.toRadians((dir+90)-(i*resol)))*resultado[i] <>
{
minim = (int)(Math.sin(Math.toRadians((dir+90)-(i*resol)))*resultado[i]);
}
}
else
{
if (Math.sin(Math.toRadians(90-(dir-i*resol)))*resultado[i] <>
{
minim = (int)(Math.sin(Math.toRadians(90-(dir-i*resol)))*resultado[i]);
}
}
}
}
return minim;
}
Esta función nos devuelve la mínima distancia que realmente puede recorrer la la dirección theta proporcionada.

Sin embargo como podemos ver, la complejidad para calcular todo esto es muy grande (tiende a O(n^2)) y el problema es que requiere muchos cálculos y memoria, y cuando lo metimos en el ladrillo del NXT no funcionaba, debido a que se quedaba sin memoria.
Poniéndolas como globales conseguíamos que funcionara, pero tardaba unos 35s.
De hecho hicimos que diera un beep cada vez que terminaba una dirección (hay 37 con resolución a 5º), los 10 primeros iban más o menos rápidos, el resto tardaban cada vez más porque ya se había quedado sin memoria.
Después nos dimos cuenta que quizá no necesitábamos tantas medidas (cada 5º). Y entonces a lo mejor había funcionado.

Probamos la función en el ordenador, con los datos proporcinados por el sensor y traspasados por USB y funciona perfectamente y decide no atravesar puertas pequeñas demasiado cerca.

Aquí tenemos una muestra de que funciona:



En el vídeo, aunque no avanza porque está desactivado, podemos ver como teniendo un buen hueco, no escoge esa dirección porque no cabe.

El radar: Al detalle

Vamos a especificar algunos detalles técnicos de la práctica final, y de la parte de navegación que hemos hecho estos días.

Para mover el sensor de ultrasonidos, estamos utilizando el método rotateTo en lugar de rotate.
En principio los dos métodos son iguales, pero eso sólo ocurriría en un motor ideal. Sin embargo, se pueden producir pequeños errores en el movimiento. Si le decimos que rote 20º, puede girar entre 18º-22º (por poner un ejemplo).
En principio esto nos daría igual, pero lo malo es que estos errores son acumulativos. Hicimos una prueba, y con rotate, si girábamos 180º en tomas de 1º, acababa girando unos 230º, mucho error.

Sin embargo con rotateTo esto no ocurre porque, a pesar de tener el mismo margen de error, este no es acumulativo. Si giramos a la posición 5º, y luego queremos girar a la posición 10º, da igual si al principio se quedó en 3º, en 5º, en 6º, o en 20º. Irá a la posición 10º, con las mismas posibilidades de error, pero no acumuladas.

Para la impresión por pantalla utilizamos trigonometría básica, teníamos la hipotenusa, teníamos el ángulo, hallar los dos catetos estaba tirado.
La escala la hicimos utilizando las constantes de LCD.SCREEN_WIDTH/2 y LCD.SCREEN_HEIGHT.
El ancho lo dividimos entre dos porque hemos colocado el robot en medio, y la medida más larga será la mitad. En este caso además, debíamos sumarle LCD.SCREEN_WIDTH, para que los negativos pasaran a ser la parte izquierda y los positivos la parte derecha de la pantalla.
El único problema que tuvimos aquí fue acordarnos de los radianes, hasta que nos dimos cuenta, las medidas se iban dando vueltas por la pantalla.

El tema de la navegación ya es algo más complicado.
Al principio nos definimos varias constantes, entre ellas la resolución a utilizar, la distancia mínima, y el ángulo base.

La distancia mínima es una distancia de seguridad, que nos permite un cierto recorrido sin chocar.
El ángulo base, es un ángulo que definimos para dividir el espacio. Estos se solapan, es decir, con una resolución de 5º, y un ángulo base de 20º, el primero va de 0 a 20, el segundo de 5 a 25, etc.

Después de tomar las medidas, aplicamos un filtro, para eliminar ciertas medidas "extrañas", típicamente una muy alta (en torno a los 255, que también puede indicar un error) entre dos más discretas.

Seguidamente asignamos una nota a estos espacios en función de si cumplen la distancia mínima y de cuanto puede recorrer el robot. Actualmente utilizamos la media para asignar las notas, funciona bien, aunque nos estamos planteando utilizar el mínimo.

A partir de ahora trabajamos con estas notas. Detectamos cual es la mejor y buscamos los grupos seguidos más grandes. Esto es, si la nota máxima es de 45, buscamos el conjunto seguido de notas máximas. En este caso, para admitir pequeñas variaciones, aceptamos en el grupo notas un 10% más bajas que la máxima, en este caso, más de 41. Podríamos bajar un poco más, aunque de momento funciona bien, las pruebas dirán. Pero sí es importante para medidas altas, normalmente 255, para admitir desde 220.
Hay que tener en cuenta que hablamos de notas máximas, no de medidas máximas. ¡Siempre notas! De no ser así tendríamos problemas con picos en las medidas.

Una vez tenemos el inicio del mayor grupo y su tamaño, escogemos como mejor dirección la que esté en medio.
Para saber cuanto podemos andar buscamos la medida mínima en un rango de la mitad del ángulo base a cada lado (con un ángulo de 20º, 10º a cada lado). Estamos pensando en ampliar este márgen para que detecte bien las cosas, pero tendríamos que tener en cuenta que podemos salirnos del rango de medidas que tenemos. Sin embargo estamos esperando a ver los resultados de otra función que tenemos pensada para ver si aplicamos esta subida o no.

La función tratará de evitar que choquemos si, por ejemplo, cerca de una "puerta" en la que tenemos un espacio de 20º o más en el que no detectamos nada, pero en el hueco no entramos.
¡Iremos dando más detalles!

También hemos implementado una conexión USB para pasar todos los datos al ordenador. Nos dió problemas en los ordenadores de la universidad y acabamos intentandolo en casa.
También hubo muchos problemas porque no encotraba los drivers, hasta que después de googlear muchísimo y mirar las implementaciones de las comunicaciones, encontramos una entrada en el foro de Introducción a la Robótica de Carlos Agüero en el que explicaba que debíamos copiar el archivo libjlibnxt.so en /usr/lib. En lugar de copiar hicimos un enlace simbólico, y funcionó sin tocar nada más.

Sin embargo para hacer lo mismo con bluetooth no hemos sido capaces. Hemos mirado muchísima documentación, hemos probado muchas cosas, como hacer lo mismo con libjbluez.so, con los archivos de $NXT_HOME/3rdparty/lib (libbluecove.so y demás, extraidos de bluecove-gpl.jar) utilizar el fichero de configuración para cambiar a bluez, para cambiar a bluecove... nada ha dado resultado (a pesar de que nxjupload y nxjbrowse funciona por bluetooth).
Si no fuera por esto, lo tendríamos funcionando, y además seleccionando la conexión (USB o bluetooth).

Siguiendo bien las líneas con comportamientos

Después de andar trasteando un poco y aunque al final conseguimos solucionar la práctica de los comportamientos, se nos quedo una espinita clavada con el maldito "ismoving". Última solución, mirar los fuentes.
Hemos revisado el código fuente de Arbitrator, Navigator, Motor y BasicMotor. Resulta que los motores tienen en principio tres (que nos importen) estados, FORWARD, BACKWARD y STOP. Su ismoving significa simplemente que no este ni en FORWARD ni en BACKWARD (puede estar también en estado FLOAT, pero este no le utilizamos, en este caso el motor va suelto, se deja que pare el solo).
Sin embargo, en Motor, añaden la funcionalidad de rotateTo, que luego utiliza Navigator.
Para esto añaden una variable booleana _isrotating. Y su ismoving es que no este ni FORWARD ni BACKWARD y que además _isrotating sea falso.
El método Stop actualiza el estado del motor a Stop y se supone que siempre que se cambia un estado, _isrotating es puesto a false.
El problema es que tienen un Thread corriendo, que se encarga de que si se pasa rotando, de la vuelta, y así indefinidamente, aunque con ciertos límites. Este Thread cambia velocidades, modos (FORWARD, BACKWARD y STOP) y hace un montón de cosas, un poco complicado.

Nuestro problema (comprobado y depurado) es que no salía de un bucle porque se quedaba parado (al ejecutarse un supress) y no estaba en la línea negra, sin embargo ismoving seguía devolviendo true. Probamos cambiando el supress de avanzar (que era parar cada motor por separado) por navigator.stop; pero esto tampoco funciona (un rápido vistazo del código muestra que parar cada motor es lo que hace navigator.stop). Lo que dedicimos al final fue no utilizar el método ismoving de motor, sino el metodo isStopped (que pertenece a BasicMotor) del motor que movemos. De esta forma, pasamos del _isrotating, lo que en principio sería peligroso, aunque no tanto, porque para rotar lo hace con FORWARD y BACKWARD.

Y bueno, parece que funciona:



Y por cierto, algo curioso. El método suppress no se ejecuta siempre, sino solo cuando cambiamos a un comportamiento con más prioridad. Si pasamos por ejemplo del comportamiento cero del array, al uno, no se ejecuta ningún supress.

Práctica final: El radar

Ahora Bender se está convirtiendo en una temible máquina de guerra (aunque de momento como radar).

No queremos dar muchos detalles por el momento, ya que es la práctica final y aún estamos empezando. De momento ya hemos logrado que haga un barrido y lo muestre por pantalla (la clase Radar). Lo más difícil, y a lo que más tiempo hemos dedicado ha sido a montar el sensor (en las 2h de clase lo teníamos programado, excepto por un detalle que ya comentaremos -Mu tonto-), el diseño ha resultado ser extremadamente sencillo y ligero, cumpliendo perfectamente su cometido. Así funciona:




Y este es el resultado:


El "agujero" que se ve (entre donde estoy yo, y donde apunta a la puerta, esa especie de linea que se aleja al limite situada casi en el centro) es consecuencia de mi brazo o mi mano sujetando la cámara.

Siguiendo lineas

Esta práctica ha sido con toda seguridad la más difícil hasta ahora.

Se trata de seguir una línea negra sobre fondo blanco, hasta ahí bien. El problema es utilizar los comportamientos de leJos.
Desde un principio pensamos que lo mejor sería utilizar dos comportamientos:
1. Avanzar, siempre takeControl siempre devuelve verdadero. Lo único que hace es poner la velocidad adecuada y avanzar. Al salir detiene los motores.
2. Girar, takeControl devuelve verdadero cuando detecta blanco en el sensor. En la clase principal lo hemos calibrado, 0->negro, 100->blanco. Como sabemos que varía un poco, le pusimos verdadero siempre que detecta más de 80.
Lo que hace es girar 10º a un lado y a otro, 20º después, 30º, 40º, ... de esta forma encontramos el más cercano. Como normalmente estará cerca, no perderemos demasiado tiempo. Al salir detiene los motores.

La implementación fue sencilla, pero llegaron los problemas. Aún seguimos sin saber por qué, pero una vez que se metía en un comportamiento, y a pesar de que el arbitro sabía que debía darle el control a otro (eso indicaban los takeControl), nunca cambiaba. De esta forma, como calibramos primero el blanco y luego el negro, simplemente avanzaba como si no hubiera un mañana.
Para nuestra desesperación, a pesar de las multiples trazas, lo único que hacía que cambiase el control era poner sleeps después de las ejecuciones. Eso nos daba un plazo (de la misma duración que el sleep) para cambiar. Si se pasaba ese plazo, vuelta a lo de antes.

Como último intento, copia&pega&colorea del ejemplo del enunciado. Lo copiamos, funcionaba, y lo retocamos para que se comportase para seguir líneas (copiar&pegar del proyecto anterior) y... sorpresa!! FUNCIONA!! Estuvimos planteándonos colgar los dos códigos y ofrecer una recompensa por encontrar las 7 diferencias, pero si son iguales!!

Ahora ya sólo quedaba resolver algún problemilla, y es que de vez en cuando, el robot se paraba al salir de la línea, y sólo continuaba si volvías a meterlo. Unas trazas y pruebas más tarde, descubrimos el problema.
Para realizar la acción de girar, se giraba con steer, retornando inmediatamente, y para conseguir parar si encontraba negro, nos metíamos en un bucle con un sleep. El bucle sólo finalizaba al encontrar negro o cuando la rueda dejaba de moverse.
El problema era que si se ponía a girar, e inmediatamente encontraba negro, se ejecutaba alguna instrucción de las de parada (en los supress) y detenía las ruedas. Sin embargo, hasta que no termina la acción de girar, no ejecuta la acción de avanzar, y esta nunca termina porque sorprendetemente, ismoving seguía retornando verdadero, y como la luz no cambia, ahí se queda.

Tuvimos que rediseñarlo completamente para eliminar estos sleep, y ahora gira de 5º en 5º y comprueba la luz.





Como no podemos subir archivos, escribo aquí el código utilizado:

import lejos.nxt.*;
import lejos.subsumption.*;

public class SeguirLineas
{
public static void main(String [] args) {

LightSensor LuzP3 = new LightSensor (SensorPort.S3);

LuzP3.setFloodlight(true);

LCD.clear();
LCD.drawString("Calibrar",3,4);
LCD.drawString("alto",5,6);
Button.waitForPress();
LuzP3.calibrateHigh();

LCD.clear();
LCD.drawString("Calibrar",3,4);
LCD.drawString("bajo",5,6);
Button.waitForPress();
LuzP3.calibrateLow();


Behavior b1 = new Avanza();
Behavior b2 = new Gira(LuzP3);
Behavior [] bArray = {b1, b2};
Arbitrator arby = new Arbitrator(bArray);
arby.start();
}
}



import lejos.subsumption.*;
import lejos.nxt.*;
import lejos.navigation.*;

public class Gira implements Behavior
{
public LightSensor luz;
private Pilot navigator = new Pilot(5.6f, 11.25f, Motor.A, Motor.C);

public Uno (LightSensor sensor)
{
luz=sensor;
}

public boolean takeControl() {
//Consideramos blanco más de un 80% de luz (sobre lo calibrado)
LCD.clear();
LCD.drawInt(luz.readValue(), 3, 4);
if (luz.readValue()>80)
LCD.drawString("true", 3, 6);
else
LCD.drawString("false", 3, 6);

return (luz.readValue()>80);
}
public void suppress() {
navigator.stop();
}

public void action() {
navigator.stop();
navigator.setSpeed(180);

for (int i=10; i<180>=20; i=i+10)
{
MoverPeroParar ( 2*i-10);

if (luz.readValue()>=20)
{
MoverPeroParar (-2*i);

}
}

navigator.stop();
}

private void MoverPeroParar (int cantidad)
{
int contador=0;

if (cantidad>0)
{
while (luz.readValue()>=20 && contador=20 && contador>cantidad)
{
contador = contador-5;
navigator.steer(200,-5,true);
try {Thread.sleep(20);} catch (Exception e) {}
}
}
}

}


import lejos.subsumption.*;
import lejos.nxt.*;

public class Avanza implements Behavior
{
public boolean takeControl()
{
return true;
}

public void suppress() {
Motor.A.stop();
Motor.C.stop();
}

public void action() {
Motor.A.setSpeed(360);
Motor.C.setSpeed(360);
Motor.A.forward();
Motor.C.forward();
}

}

Practica Opcional: Proyectos en la red

Hoy hemos estado buscando distintos proyectos en internet sobre el robot Lego. El que más nos ha sorprendido ha sido un robot que era capaz de resolver un cubo de rubik en menos de un minuto!
El creador de este proyecto es Daniele Benedettelli un ingeniero industrial italiano.
Lo más curioso de este robot es que usa muy pocas piezas, algunos datos técnicos que hemos encontrado han sido los siguientes:
- Un servomotor actúa de junta prismática que empuja el lado del cubo
- Un servomotor rota la plataforma giratoria del robot
- Un servomotor tiene una doble función para ayudar el brazo empujador a acomodar el brazo en el soporte y para sujetar el cubo mientras la base inferior rotatoria gira.
- Dos sensores de contacto como detectores de límite de los brazos
- Un sensor de luz para encontrar la posición inicial de la base rotatoria.

Pero mejor verlo vosotros mismos:

Esquivando con multihilo

Esta práctica consiste en usar dos threads distintos para que el robot esquive objetos de una forma simple.

Tenemos un hilo para cada rueda. La derecha siempre está avanzando, mientras que la izquierda detecta si hay un objeto a menos de 50 cm (30 de Bender) del sensor de ultrasonidos utilizado. En ese caso detiene la rueda durante 200 ms. En caso contrario avanza.

De esta forma Bender girará a la izquierda siempre que encuentre un objeto a menos de 30 cm.

Un día en el campo

Hoy Bender se ha dado una vueltecita por el campo.



Ha jugado con la fauna de la zona...


y ha comprobado que no es un robot del campo:

Recorrido en ocho

Para esta práctica Bender debía hacer un recorrido en forma de ocho utilizando la clase Pilot.
Lo que no nos quedo muy claro es si tenía que recorrer 1 metro en forma de ocho, o sí debía recorrer un metro recto, hacer un lazo, recorrer otro metro, y hacer el otro lazo. Así que lo hicimos de las dos formas, así no hay confusión.

Lo más difícil en esta práctica eran los lazos. Empezando por el ocho corto, debía recorrer en los 360º, 50 cm. Sin embargo no tenemos ninguna forma explícita de saber eso. Hicimos una prueba utilizando steer, y con un cálculo un poco a ojo, conseguimos que recorriera 52 cm.



Para el ocho largo era más complicado, porque al recorrer más distancia, debía estar más ajustado. El recorrido sería avanzar 1 metro, girar 270º (tres cuartos), avanzar otro metro, y volver a girar otros 270º en el sentido contrario. Calculamos cuanto debía recorrer en esos 270º->235.62. Finalmente recorre 239 y acaba prácticamente en el mismo sitio desde el que partió.

Buscando "La Luz"

Bender ha abierto un poco los ojos y ahora es capaz de distinguir la luz y la oscuridad. Tanto le ha gustado este nuevo sentido, que la busca ansiosamente.

Comienza a caminar, y si encuentra un salto un poco brusco, y detecta menos luz, dará un paso atrás y se girara hacia cualquier lado una cantidad aleatoria, si en ese nuevo lugar hay aún menos luz que antes, volverá a girar, así hasta que encuentre más luz que en el último giro.

Pero eso no es todo. Si después de un rato avanzando nota que pierde demasiada luz, se dará media vuelta por donde ha venido.

Observémoslo:

Práctica 2: El rebotador

¡BENDER QUIERE SER LIBRE!

Pero antes necesita aprender algunas cosas...



Caca... por las paredes no se sube



¡Así sí!

Práctica 1: Enséñamelo todo

Este es Bender algo más funcional, de momento ya se mueve con sus tres motores.
Tiene tres opciones, rotar 45º las ruedas hacia adelante, moverse continuamente adelante, o moverse continuamente atrás. El botón escape sale del programa. Para parar sin salir, en este caso hay que rotar.
Además Bender nos enseña lo que está haciendo en cada momento mediante mensajes en la pantalla.

¡Cuidado que se escapa!

Primeros pasos de Bender

¡Estos son sus primeros pasos! (eso sí, de 45 en 45º por pulsación)



Ahora una cuestión peliaguda en el grupo; el muñeco ¿es chico o chica?
Difícil decisión, que se ha resuelto de una forma justa:

Durante la construcción del robot, es un obrero, se pone el casco y es un machote de los que tiran piropos




Pero en los días de gala, se quita el casco y se suelta la melena, ¡será una hermosa princesita!

De momento...

...los circuitos de Bender

Héchale un vistazo a mi perfil, ¡cacho carne!