/* * Problem #4, CS154 Spring 2001 * Name : SooYoung Jung * Date : April, 30, 2001 * File : nomad.cc * */ #include "GL/glut.h" #include #include #include #include #include #include "KbdInput.hh" extern "C" { #include "Nclient.h" } #define WIDTH 160 #define HEIGHT 120 /* * This is a global variable used to hold images * The "3" refers to the three color bands available. * * Each pixel has a red (0) * green (1) * and blue (2) value. * * Each of these values is between 0 and 255, inclusive. * * GLubyte is OpenGL's unsigned int. */ GLubyte im[HEIGHT][WIDTH][3]; char imageFileName[100]; // a string to hold the image file name int currentImageNumber = 0; // the image numbers go up by 6 each frame int continuousPlay = 0; // for continuous sequence playing /* * These are forward declaration of functions used later */ void getImage(int imageNumber); // loads the next image into gloabl "im" void init(); // OpenGL initialization void display(); // OpenGL general display routine void reshape(int w, int h); // OpenGL window-resize routine void mouse(int button, int state, int x, int y); // mouse-event handling void keyboard(unsigned char key, int x, int y); // keyboard-event handling /* * This is the main routine; it sets up the OpenGL * settings for displaying images. In reality this * is an event-driven program, so that the control * comes from the routines * * keyboard(key,x,y) * * and * * mouse(button,state,x,y) */ int main(int argc, char* argv[]) { SERV_TCP_PORT = 7077; // this is the default tcp port /* * tv is the constant translational velocity of the robot */ int tv = 200; /* five inches per second default */ if (argc > 1) /* a different velocity can be sent in */ { tv = atoi(argv[1]); cout << "Setting translational velocity to " << tv << endl; } /* * connect to the simulator */ if (connect_robot(1)) { printf("Connected to robot.\n"); printf("Hit q to exit.\n"); } else { printf("Failed to connect to robot\n"); exit(0); } /* * zero the robot */ zr(); /* * make the robot to see front at the beginning */ pr(0, 900, 900); ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[39] != 0) gs(); /* * turn on appropriate sensors */ for (int i=17 ; i<=32 ; ++i) Smask[i] = 1; ct(); gs(); /* update the robot's state */ /* * the KbdInput object allows nonblocking input */ KbdInput KI; char c; /* * the sense/act loop implementing reactive control */ KI.on(); /* turn on raw input/output */ while (1) { c = KI.nextChar(); /* c is 0 if there's no input ready */ if ( c == 'q' ) /* hit q in the terminal window to quit */ { break; } /* * If you're curious as to what is going on in OpenGL, * one good source of information is http://www.opengl.org */ /* set up window */ glutInit(&argc, argv); glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE ); glutInitWindowSize(160,120); glutInitWindowPosition(100,100); glutCreateWindow("Object Tracking"); /* initialize state */ init(); /* register enevt-handling functions */ glutDisplayFunc(display); glutReshapeFunc(reshape); glutMouseFunc(mouse); glutKeyboardFunc(keyboard); /* wait until something happens */ glutMainLoop(); gs(); /* update state */ } KI.off(); /* reset terminal settings */ disconnect_robot(1); /* gracefully disconnect from the robot */ return 1; } /* * This routine provides additional initialization for * various OpenGL parameters. */ void init() { /* sets clear color to black */ glClearColor(0.0,0.0,0.0,0.0); /* define the viewing system */ glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluOrtho2D(0.0, (GLfloat)WIDTH, 0.0, (GLfloat)HEIGHT); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glShadeModel(GL_FLAT); glPixelStorei(GL_UNPACK_ALIGNMENT,1); } /* * A routine to get the next image from the files * in /cs/cs154/as/problemset4/redSequence */ void getNextImage() { /* * set up the next file to read */ sprintf(imageFileName, "/mnt/cs/cs154/as/problemset4/redSequence/frame.%04d.ppm", currentImageNumber); cout << "Reading image " << imageFileName << endl; ifstream inf(imageFileName); /* * There is a 15-byte header */ unsigned char c; for (int k=0 ; k<15 ; ++k) { inf.get(c); // cout << "k is " << k << " and c is " << (int)c << " " << c << endl; } /* * and then we get to the image... */ for (int j=HEIGHT ; j>=0 ; --j) for (int i=0 ; i 0 ? y-4 : 0; int ymax = y+4 < HEIGHT ? y+4 : HEIGHT-1; int xmin = x-4 > 0 ? x-4 : 0; int xmax = x+4 < WIDTH ? x+4 : WIDTH-1; for (int j=ymin ; j<=ymax ; ++j) im[j][x][0] = im[j][x][1] = im[j][x][2] = 255; // set r,g,b to max for (int j=xmin ; j<=xmax ; ++j) im[y][j][0] = im[y][j][1] = im[y][j][2] = 255; // set r,g,b to max } /* * Here is processTheImage() -- this is the function * you should use in order to track the red frisbee from * frame to frame. (Certainly you should feel free to * add other functions that will be called from this one.) * * You may keep any state about the frisbee around (OpenGL * tends to use global variables, though you can use static * variables in this function if you're very opposed to * globals. * * For example, you may want to keep the position of the frisbee * in the previous frame as a starting point for locating it in the * next one. * * In order to indicate where your algorithm believes the * frisbee is, you should draw a white + sign at the * location estimate you compute. As a start, the routine * currently draws a white + sign in the middle of the window. * * Feel free to draw (or print out to the console) whatever other * graphics (or textual information) you think might be helpful. * * Keep in mind that you can program keyboard input and mouse * input to help you automate tasks (in the "keyboard" and "mouse" * functions below. * */ /* * If tracking is first time, then set the search region to be the entire * image. Otherwise, previous center coordinate will be used to get the * search region. */ bool firstTime = true; // Whether we are tracking the first image int xCenter = 0; // center point of the Frisbee int yCenter = 0; int maxImageMove = 30; // Assume the Frisbee will not travel more // than 10, ( + 20 = 30, 20 is the size of the // Frisbee) pixel per frame /* * findRed will find the red pixel and add them up * average will be used as the center of the frisbee */ void findRed() { int xSum = 0; // total x and y int ySum = 0; int count = 0; int R, G, B; int iStart, iEnd; int jStart, jEnd; /* If the we are tracking first time, then search entire image */ if (firstTime) { iStart = 0; iEnd = WIDTH; jStart = 0; jEnd = HEIGHT; firstTime = false; } else { /* limit search area */ iStart = ((xCenter - maxImageMove) < 0 ? 0 : (xCenter - maxImageMove)); iEnd = ((xCenter + maxImageMove) > WIDTH ? WIDTH : (xCenter + maxImageMove)); jStart = ((yCenter - maxImageMove) < 0 ? 0 : (yCenter - 20)); jEnd = ((yCenter + maxImageMove) > HEIGHT ? HEIGHT : (yCenter + maxImageMove)); } /* * Checking search region. add up if the pixel is red */ for (int j = jStart; j < jEnd; j++) for (int i = iStart; i < iEnd; i++) { R = (int)im[j][i][0]; G = (int)im[j][i][1]; B = (int)im[j][i][2]; /* * I defined this is red in the pictures */ if ((R > 128 && R < 180) && (G > 32 && G < 64) && (B > 32 && B < 64) || (R > 181 && R < 256) && (G > 64 && G < 100) && (B > 64 && B < 100) ) { /* show what's red when i need to see what the red is, im[j][i][0] = 0; im[j][i][1] = 0; im[j][i][2] = 128; */ count++; xSum += i; ySum += j; } } /* Average point */ xCenter = xSum / count; yCenter = ySum / count; } /* * Nomad * * calculate the angle to the center of the frisbee and the center of the * image of the bottom * After getting the angle, turn the nomad * * Stolen this function from the previous assignment * - bug2, s-line follower */ float PI = 3.14159265; void nomad(int y, int x) { int steeringAngle = State[36]; int lengthB = x - (WIDTH/2); int lengthC = HEIGHT - y; /* * Result will be inverse tan of angle. */ double angle = (90 - (atan2(lengthB, lengthC) * 180 / PI)) * 10; /* * Just need to turn differences between previos and current */ angle -= steeringAngle; pr(0, angle, angle); ws(TRUE, TRUE, TRUE, TRUE); /* wait for turing */ while (State[39] != 0) gs(); } void processTheImage() { /* The image is the global variable "im" */ /* * some code to seek out red pixels and find * an estimate of their center should go here */ /* Right now, we estimate that the frisbee is in the center of the image */ /* * firstly find the red pixels and average them to determine the center * of the frisbee. and draw the center using the function drawPlus, * which was given */ findRed(); drawPlus(yCenter, xCenter); /* * make the nomad stare the center of the frisbee */ nomad(yCenter, xCenter); /* * this is my first attemp, but this takes forever.. * and consumes about 90% CPU of turing machine * just leave this for make this program smart later * int FRISBEE = 60; int percentMax = 0; int xmax, ymax; int percent; for (int j = 0; j < HEIGHT; j++) for (int i = 0; i < WIDTH; i++) { percent = percentRed(j, i); if (percent > FRISBEE) if (percent > percentMax) { xmax = i; ymax = j; } } drawPlus (ymax, xmax); cout << "xmax:" << xmax << " ymax:" << ymax << " percentMax:" << percentMax << endl; cout << "first done" << endl; */ } /* * I don't need these functions * clear(), drawCircle(), checkRed(), percentRed() */ /* * change all the pixel to black, * to check the circle is drawing well or not. */ void clear() { for (int i = 0; i < HEIGHT; i++) for (int j = 0; j < WIDTH; j++) im[i][j][0] = im[i][j][1] = im[i][j][2] = 0; } /* * Draw a circle in the image. the size of the circle is about the size of * frisbee * */ int RADIUS = 18; /* Radius of frisbee */ void drawCircle(int y, int x) { drawPlus(y, x); int ymin = y-RADIUS > 0 ? y-RADIUS : 0; int ymax = y+RADIUS < HEIGHT ? y+RADIUS : HEIGHT-1; int xmin; int xmax; for (int i = ymin; i <= ymax; i++) { xmax = (int)sqrt((RADIUS * RADIUS) - ((i-y) * (i-y))); xmin = (x - xmax) < 0 ? 0 : (x - xmax); xmax = (x + xmax) > WIDTH ? WIDTH -1 : (x + xmax); im[i][xmin][0] = im[i][xmin][1] = im[i][xmin][2] = 255; im[i][xmax][0] = im[i][xmax][1] = im[i][xmax][2] = 255; } } bool checkRed(int y, int x) { int R, G, B, R_B, R_G; R = (int)im[y][x][0]; G = (int)im[y][x][1]; B = (int)im[y][x][2]; R_G = R-G; R_B = R-B; if ((R > 100 && R < 180) && (G > 30 && G < 80) && (B > 30 && B < 80) || (R_G > 50 && R_B > 50) ) { return true; } return false; } int percentRed(int y, int x) { int ymin = y-RADIUS > 0 ? y-RADIUS : 0; int ymax = y+RADIUS < HEIGHT ? y+RADIUS : HEIGHT-1; int xmin; int xmax; int count = 0; int Rcount = 0; for (int i = ymin; i <= ymax; i++) { xmax = (int)sqrt((RADIUS * RADIUS) - ((i-y) * (i-y))); xmin = (x - xmax) < 0 ? 0 : (x - xmax); xmax = (x + xmax) > WIDTH ? WIDTH - 1 : (x + xmax); for (int j = xmin; j <= xmax; j++) { if (checkRed(i, j)) { Rcount++; } count ++; } } float result = (Rcount / (float)count) * 100; return (int)result; } /* * The following function gets called in response to * a glutPostRedisplay() call. */ void display() { glClear(GL_COLOR_BUFFER_BIT); getNextImage(); // gets the next image from file /* * Here is the routine in which you should put all * of your processing. Because OpenGL tends to use * global variables to hold state (even though this * would not pass muster in CS70), I would encourage * it for this assignment. */ processTheImage(); glRasterPos2i(0,0); glDrawPixels(WIDTH,HEIGHT,GL_RGB,GL_UNSIGNED_BYTE,im); // draws it glutSwapBuffers(); // used to support double buffering } /* * an unused function for reshaping the window when * a user resizes it */ void reshape(int w, int h) { /* width and height store current window dimensions */ /* map the projection plane to the entire window */ // glViewport(0,0,WIDTH,HEIGHT); /* * We ignore reshape commands and assume the user will * not try to resize the window. */ } void idlePlay() { glutPostRedisplay(); } /* * function to handle keyboard inputs * * right now, q -- quits * n -- moves to next frame * p -- toggles continuous image updating */ void keyboard(unsigned char key, int x, int y) { switch (key) { /* * I cannot find a glut function that * lets you more cleanly exit the loop. * Several online glut references say simply * to call exit, so we will... . */ case 'q': case 'Q': exit(0); break; case 'n': case 'N': glutPostRedisplay(); break; case 'p': case 'P': continuousPlay = 1 - continuousPlay; if (continuousPlay) glutIdleFunc(idlePlay); else glutIdleFunc(NULL); break; default: cout << "Key pressed was " << key; cout << " at (" << x << "," << y << ")\n"; break; } return; } /* * function to handle mouse inputs * currently unused, but the mouse events are * printed to make it simple to incorporate, if * you'd like to */ void mouse(int button, int state, int x, int y) { /* if (button == GLUT_LEFT_BUTTON) { cout << "Left "; } if (button == GLUT_MIDDLE_BUTTON) { cout << "Right "; } if (button == GLUT_RIGHT_BUTTON) { cout << "Right "; } if (state == GLUT_DOWN) { cout << "button pressed "; } if (state == GLUT_UP) { cout << "button released "; } cout << "at (" << x << "," << y << ")\n"; */ if (state == GLUT_DOWN) { cout << "Pixel at (" << x << "," << (y) << ") is ( r, g, b ) = ("; cout << (int)im[HEIGHT-y][x][0] << ", "; cout << (int)im[HEIGHT-y][x][1] << ", "; cout << (int)im[HEIGHT-y][x][2] << " )\n"; } /* drawCircle((HEIGHT - y), x); redraw(); */ return; }