/*! 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; }