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) &&
{
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 var
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é:
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
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:
- Choques inminentes
- Centrarse en el pasillo
- 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.
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).
No hay comentarios:
Publicar un comentario