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.
jueves, 4 de junio de 2009
martes, 2 de junio de 2009
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");
}
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");
}
lunes, 1 de junio de 2009
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:
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).
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).
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.
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.
Suscribirse a:
Entradas (Atom)