/*!
Track and compute the dot characteristics.
To get the center of gravity coordinates of the dot, use
getCog(). To compute the moments use setComputeMoments(true) before
a call to initTracking().
\warning The image is modified (all the pixels that belong to the point
are set to white (ie to 255).
\param I : Image to process.
\sa getCog()
*/
void
vpDot::track(const vpImage<unsigned char> &I)
{
try{
setGrayLevelOut();
double u = this->cog.get_u();
double v = this->cog.get_v();
COG( I, u, v ) ;
this->cog.set_u( u );
this->cog.set_v( v );
if (compute_moment==true)
{
mu11 = m11 - u*m01;
mu02 = m02 - v*m01;
mu20 = m20 - u*m10;
}
if (graphics) {
// display a red cross at the center of gravity's location in the image.
vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
}
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
}
/*!
Track and compute the dot characteristics.
To get the center of gravity coordinates of the dot, use
getCog(). To compute the moments use setComputeMoments(true) before
a call to initTracking().
\warning The image is modified (all the pixels that belong to the point
are set to white (ie to 255).
\param I : Image to process.
\sa getCog()
*/
void
vpDot::track(vpImage<unsigned char> &I)
{
try{
setGrayLevelOut();
double u = this->cog.get_u();
double v = this->cog.get_v();
COG( I, u, v ) ;
this->cog.set_u( u );
this->cog.set_v( v );
if (compute_moment==true)
{
mu11 = m11 - u*m01;
mu02 = m02 - v*m01;
mu20 = m20 - u*m10;
}
}
catch(...)
{
vpERROR_TRACE("Error caught") ;
throw ;
}
}
int main( int argc, char** argv )
{
/*
* serial stuff
*/
int fd = 0;
char serialport[256];
int baudrate = B19200;
//FIXME
fd = serialport_init("/dev/ttyUSB0", baudrate);
if(fd==-1) return -1;
usleep(3000 * 1000);
///////////////////////////////////////////////////////
int c = 0, fps = 0;
//capture from camera
CvCapture *capture = cvCaptureFromCAM(1);
//quit if camera not found
if(!capture) {
printf("cannot init capture!\n");
return -1;
}
//display original video stream
cvNamedWindow("stream", CV_WINDOW_NORMAL);
cvResizeWindow("stream", 320, 240);
cvNamedWindow("hue", CV_WINDOW_NORMAL);
cvResizeWindow("hue", 320, 240);
cvMoveWindow("hue", 320, 0);
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2, CV_AA);
//keep capturing frames until escape
while(c != 27) //27 is escape key
{
//quit if can't grab frame
if(!(frame = cvQueryFrame(capture))) break;
//show the default image
//cvShowImage("stream", frame);
//edge detection - todo: HSV color filtering
findLine(frame);
//edge detection
findEdge(frame);
if(flag == 2)
cvPutText(frame, "right edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
else if(flag == 1)
cvPutText(frame, "left edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
//display center of gravity in coordinates
COG(&cog_x, &cog_y);
char x_coord[10];
char y_coord[10];
sprintf(x_coord, "%1.2f", cog_x);
sprintf(y_coord, "%1.2f", cog_y);
//find center of y coordinate in left and right edge
COG_edges(&left_y, &right_y);
//printf("%1.2f\n", left_y);
//printf("%1.2f\n", right_y);
//find slant
findSlant(frame);
char write_slant[15];
if(slant && offset) sprintf(write_slant, "%s", "crooked slant!"); //eventually invokes s movement
else sprintf(write_slant, "%s", "\0");
//TODO: FIXME
//motor1 is closer to bottom, motor 2 is closer to top (in starting config)
uint8_t b = 0b00000000;
//motor logic
char motor1[10];
char motor2[10];
//handles any errors - move 0.5 second for now
//might have to flip these
if(flag == 1)
{
b = 0b00000111;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "backward");
write(fd,&b,1);
}
else if(flag == 2)
{
b = 0b00000100;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "forward");
write(fd,&b,1);
}
else if(ROR) //rotate right (eventually look at angle and change timing appropriately)
{
printf("%s\n", "ROTATE RIGHT");
sleep(2);
b = 0b00000101;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "backward");
write(fd,&b,1);
}
else if(ROL) //rotate left (eventually look at angle and change timing appropriately)
{
printf("%s\n", "ROTATE LEFT");
sleep(2);
b = 0b00000110;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "forward");
write(fd,&b,1);
}
//NO ERROR - THIS HANDLES LOGIC TO DRIVE ROBOT IN CORRECT SEQUENCE
//determines the amount of time and directions in which to move the motors.
//the arduino is responsible for sending stop command to wheels!
else
{
switch(count)
{
//first shingle - already in place
case 0:
b = 0b00000000; //do nothing
write(fd,&b,1);
//sleep(6); //however long it takes to place shingle, or wait until arduino says go
printf("%s\n", "case 0");
break;
//second/third shingle - drive forward one foot (for now 2 secs, figure this out!)
case 1:
case 2:
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
sleep(3);
printf("%s\n", "case 1/2");
break;
//left (viewing from bottom) overhang
case 3:
b = 0b00100011; //backward 1/2 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b00100011; //backward 1/2 foot
write(fd,&b,1);
//sleep(12);
printf("%s\n", "case 3");
break;
//right overhang
case 4:
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
//sleep(14);
printf("%s\n", "case 4");
break;
//next to right overhang
case 5:
b = 0b01000011; //backward 1 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
//sleep(8);
break;
//next to left overhang
case 6:
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
//sleep(6);
break;
//reposition at beginning position
case 7:
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b00100000; //forward 1/2 foot
write(fd,&b,1);
b = 0b00110010; //ROL 90 degrees
write(fd,&b,1);
b = 0b01100011; //backward 1+1/2 foot
write(fd,&b,1);
//sleep(14);
break;
//should not get here
default:
b = 0; //error
write(fd,&b,1);
//sleep(20);
break;
}
//count = (count+1)%8; //FIXME
}
//SERIAL - motor logic
//xxxx_xxyy -> y = motor1/motor2, 0 = forward, 1 = backward, x = time in ms
//edges
//write(fd,&b,1);
//cvPutText(frame, x_coord, cvPoint(30,300), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "y:", cvPoint(0,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, y_coord, cvPoint(30,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor1:", cvPoint(0,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor1, cvPoint(150,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor2:", cvPoint(0,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor2, cvPoint(150,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, write_slant, cvPoint(0,50), &font, cvScalar(255, 0,
0, 0));
cvShowImage("stream", frame);
c = cvWaitKey(10);
//release images
//cvReleaseImage(&image);
//cvReleaseImage(&edge);
//cvReleaseImage(&final);
//cvReleaseImage(&frame);
//cvReleaseImage(&bw);
//cvReleaseImage(&gray);
}
//release created images
cvReleaseCapture(&capture);
return 0;
}
int main( int argc, char** argv )
{
///////////
//serial stuff
int fd = 0;
char serialport[256];
int baudrate = B19200;
//int baudrate = B115200; // default
fd = serialport_init("/dev/ttyUSB0", baudrate);
if(fd==-1) return -1;
usleep(3000 * 1000 );
///////////
int c = 0, fps = 0;
//capture from camera
CvCapture *capture = cvCaptureFromCAM(1);
//quit if camera not found
if(!capture) {
printf("cannot init capture!\n");
return -1;
}
//display original video stream
cvNamedWindow("stream", CV_WINDOW_AUTOSIZE);
cvNamedWindow("hue", CV_WINDOW_NORMAL);
cvResizeWindow("hue", 320, 240);
cvMoveWindow("hue", 640, 0);
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2, CV_AA);
//keep capturing frames until escape
while(c != 27) //27 is escape key
{
//quit if can't grab frame
if(!(frame = cvQueryFrame(capture))) break;
//show the default image
//cvShowImage("stream", frame);
//edge detection - todo: HSV color filtering
findLine(frame);
//edge detection
findEdge(frame);
if(flag == 2)
cvPutText(frame, "right edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
else if(flag == 1)
cvPutText(frame, "left edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
//display center of gravity in coordinates
COG(&cog_x, &cog_y);
char x_coord[10];
char y_coord[10];
sprintf(x_coord, "%1.2f", cog_x);
sprintf(y_coord, "%1.2f", cog_y);
//find center of y coordinate in left and right edge
COG_edges(&left_y, &right_y);
printf("%1.2f\n", left_y);
printf("%1.2f\n", right_y);
uint8_t b = 0b00000000;
//motor logic
char motor1[10];
char motor2[10];
if(flag == 1)
{
b = 0b00010001;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "backward");
}
else if(flag == 2)
{
b = 0b10011001;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "forward");
}
else if((int)(left_y/10.0) - (int)(right_y/10.0) < -4) //rotate right
{
b = 0b10010001;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "backward");
}
else if((int)(right_y/10.0) - (int)(left_y/10.0) < -4) //rotate left
{
b = 0b00011001;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "forward");
}
else
{
b = 0;
sprintf(motor1, "%s", "STOP");
sprintf(motor2, "%s", "STOP");
}
//SERIAL - motor logic
//xxxx_xxxx = motor1_motor2, 1-15 -> -7,7 -> 8 = 0 = 1000
//edges
write(fd,&b,1);
//cvPutText(frame, x_coord, cvPoint(30,300), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "y:", cvPoint(0,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, y_coord, cvPoint(30,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor1:", cvPoint(0,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor1, cvPoint(150,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor2:", cvPoint(0,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor2, cvPoint(150,200), &font, cvScalar(255, 0, 0, 0));
cvShowImage("stream", frame);
c = cvWaitKey(10);
}
cvReleaseCapture(&capture);
//avoid memory leaks
cvReleaseImage(&image);
cvReleaseImage(&red);
cvReleaseImage(&green);
cvReleaseImage(&red_edge);
cvReleaseImage(&green_edge);
cvReleaseImage(&edge);
cvReleaseImage(&final);
cvReleaseImage(&frame);
cvReleaseImage(&bw);
cvReleaseImage(&gray);
return 0;
}
