El algoritmo para hacer seguir al robot la linea roja, se basa en la división de la imagen en 3 regiones.
Hemos dividido las regiones de la siguiente manera:
si el ancho total de la imagen son 320 pixels dividiremos los lados en 130 pixels cada uno para tomar el centro con 60 pixels y así tener una buena precisión en el código.
El algoritmo iterativo lo que realiza es lo siguiente:
recogemos la imagen mediante la función:
-cogerImagen1(&image);
despues mediante las funciones detectaRojo: detectaRojoIzq,detectaRojoCent,y detectaRojoDer sabremos hacia adonde tenemos que girar o avanzar. Estas funciónes se han programado fuera de la función navega. DetectaRojo mira en aquella region todos los pixels rojo que vea para luego devolverlos.
Aqui dejamos nuestro cogido de navega.cpp.
int detectaRojIzq(unsigned char *image1)
{
int i,j;
int pixelRojos=0;
for (i=0;i<240;i++)
for (j=1;j<130;j++)//[(i*320*3)+j*3]
if ((image1[(i-1)*320*3+(j-1)*3]>250) &&
(image1[(i-1)*320*3+(j-1)*3+1]<50)&&
(image1[(i-1)*320*3+(j-1)*3+2]<50))
pixelRojos++;
return pixelRojos;
}
int detectaRojCent(unsigned char *image1) {
int i,j;
int pixelRojos=0;
for (i=0;i<240;i++)
for (j=130;j<190;j++)
if ((image1[(i-1)*320*3+(j-1)*3]>250) &&
(image1[(i-1)*320*3+(j-1)*3+1]<50)&&
(image1[(i-1)*320*3+(j-1)*3+2]<50))
pixelRojos++;
return pixelRojos;
}
int detectaRojDer(unsigned char *image1) {
int pixelRojos=0;
int i,j;
for (i=0;i<240;i++)
for (j=190;j<320;j++)
if ((image1[(i-1)*320*3+(j-1)*3]>250) &&
(image1[(i-1)*320*3+(j-1)*3+1]<50)&&
(image1[(i-1)*320*3+(j-1)*3+2]<50))
pixelRojos++;
return pixelRojos;
}
void Navega::iteracionControl () { int i;
int izq,cent,der;
// example: how to get pioneer position
CvPoint3D32f myPoint;
this->navegacion->cogerPosicion (&myPoint);
//printf ("encoders: X=%f mm, Y=%f mm, Theta=%f (grados)\n", myPoint.x, myPoint.y, myPoint.z);
// example: how to get laser readings
std::vector<float> laser;
this->navegacion->cogerLaser(&laser);
//printf("laser: %f (mm)\n",laser[90]);
// example: how to get image stream
unsigned char *image1;
this->navegacion->cogerImagen1 (&image1);
izq=detectaRojIzq(image1);
cent=detectaRojCent(image1);
der=detectaRojDer(image1);
if((der>izq)&&(der>cent))
{
printf("Gira Derecha\n");
printf("(IZQ,CENT,DER)=(%d,%d,%d)\n",detectaRojIzq(image1),detectaRojCent(image1),detectaRojDer(image1));
this->controller->setV(100.); // mm./s.
this->controller->setW(-10.);
}
if((cent>izq)&&(cent>der))
{
printf("Centro\n");
printf("(IZQ,CENT,DER)=(%d,%d,%d)\n",detectaRojIzq(image1),detectaRojCent(image1),detectaRojDer(image1));
this->controller->setV(100.); // mm./s.
this->controller->setW(0.);
}
if((izq>cent)&&(izq>der))
{
printf("Gira Izquierda\n");
printf("(IZQ,CENT,DER)=(%d,%d,%d)\n",detectaRojIzq(image1),detectaRojCent(image1),detectaRojDer(image1));
//giraIzquierad
this->controller->setV(100.); // mm./s.
this->controller->setW(+10.);
}
/* TODO: ADD YOUR ITERATION CODE HERE */
CvPoint2D32f destino;
this->navegacion->cogerDestino (&destino);
// example of pantilt movement:
//this->controller->setPT1 (-15.,0.);
//this->controller->setPT2 (-15.,0.);
}
Aqui un video de Toppyto virtual:
