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.