vision.zip_robot soccer vision

#include "vision.h"; void main(){ runVision(); } /*************************************************************** * Following method run Vision ***************************************************************/ void runVision(){ initializeVideo(); grabBitmap(); grabFrame2(); findBall(); findObjects(); closeFB(); printf("Please enter any character to exit: "); getchar(); } /*************************************************************** * Following method initialize the video stream * If there is error, return RET_ERROR. Otherwise, return RET_SUCCESS ***************************************************************/ int initializeVideo(){ //A double word (4 bytes) DWORD physInput; //Signed 32-bit integer int syncOnGreen; //A word (2 bytes) WORD videoType; //type of input video WORD standard; WORD source; printf("INITIALIZING CAMERA:\n"); // Check if the initialization succeeded if( FB_Init() == RET_ERROR ) { printf("FB_Init ERROR: Initializes FlashBus MV configuration values.\n"); error(1); return RET_ERROR; } // Initialize the video (automatic) // Note: MISCPARM_INPUTVID returns the first-found video input. // Because some hardware lines are shared, it confuses an RGB // signal that has sync on the red input for composite. if ( FB_GetMiscParm(MISCPARM_INPUTVID, (DWORD FAR *) &physInput) != RET_ERROR ){ videoType = (WORD)(physInput & 0xff); standard = (WORD)( (physInput >> 8) & 0xff ); source = (WORD)( (physInput >> 16) & 0xff ); syncOnGreen = (BOOL)( (physInput >> 24) & 0xff ); printf("Auto-detect Sucessful.\n"); } // If auto detection fails, manually assign the parameters. else { videoType = TYPE_COMPOSITE; // different for each computer standard = STANDARD_NTSC; source = 1; syncOnGreen = FALSE; printf("Auto-detect Failed.\n"); error(1); return RET_ERROR; } // Sets FlashBus MV video input configuration. if(FB_SetVideoConfig(videoType, standard, source, syncOnGreen)==RET_ERROR){ printf("FB_SetVideoConfig failed.\n"); return RET_ERROR; } // Sets the video capture area to system memory. if ( FB_VideoOffscreen(WIDTH_CAMERA, HEIGHT_CAMERA, DEPTH_CAMERA, TRUE) == RET_ERROR ){ printf("FB_VideoOffscreen() failed"); error(1); return RET_ERROR; } // Turn on video. if( FB_VideoLive(TRUE, ALIGN_ANY)==RET_ERROR){ printf("FB_VideoLive failed.\n"); error(1); return RET_ERROR; } printf("Camera is initialized successfuly.\n\n"); return RET_SUCCESS; } // end of InitVideo /*************************************************************** * Following method that will take the picture as saves it as a bitmap * Code obtained and modified from the FBus32 sample code library * If there is error, return RET_ERROR. Otherwise return RET_SUCCESS. ***************************************************************/ int grabBitmap(){ printf("CAPTURING IMAGE:\n"); // Turn on video if(FB_VideoLive(TRUE,ALIGN_ANY)==RET_ERROR){ return RET_ERROR; } // Grab 2 frames // 1 frame used for the image stored in the desktop // 1 frame used for the object detection if(FB_WaitVSync(2)){ return RET_ERROR; } // Video will be turn off by the option FLASHTYPE_NONE if(FB_VideoGrab(ALIGN_ANY, FLASHTYPE_NONE, 0, 0)==RET_ERROR){ return RET_ERROR; } // might need to change // Save the pictures into the arrays if(FB_CopyVGARect(INITIAL_X, INITIAL_Y, WIDTH_GRAB_VIEW, HEIGHT_GRAB_VIEW, (char HUGE *)PixelArray, -1, COPYDIR_FROMOFFSCREEN) == RET_ERROR){ return RET_ERROR; } // Saves the picture as a bitmap if(FB_SaveFile("FieldPic.BMP", INITIAL_X, INITIAL_Y, WIDTH_GRAB_VIEW, HEIGHT_GRAB_VIEW, FLAG, (short)SAVEFILE_FROMOFFSCREEN, NULL, -1) == RET_ERROR){ return RET_ERROR; } printf("The image of the field is saved as BMP successfully.\n\n"); return RET_SUCCESS; } // end of grabBitmap /*************************************************************** * Following method will close the FlushBus * If there is error, return RET_ERROR. Otherwise, return RET_SUCCESS ***************************************************************/ int closeFB(){ printf("CLOSING FLUSHBUS:\n"); if(FB_Cleanup()==RET_ERROR){ error(3); return RET_ERROR; } printf("FlushBus is shut down successfully.\n\n"); return RET_SUCCESS; } /*************************************************************** * grabFrame2 grabs a frame and places it into the PixelArray for * further manipulation (findObjects, etc.) ****************************************************************/ int grabFrame2(){ printf("GRABBING A FRAME:\n"); // Turn on video if(FB_VideoLive(TRUE,ALIGN_ANY)==RET_ERROR){ error(4); return RET_ERROR; } // Grab 2 frames // 1 frame used for the image stored in the desktop // 1 frame used for the object detection if(FB_WaitVSync(2)){ error(4); return RET_ERROR; } // Video will be turn off by the option FLASHTYPE_NONE if(FB_VideoGrab(ALIGN_ANY, FLASHTYPE_NONE, 0, 0)==RET_ERROR){ error(4); return RET_ERROR; } // might need to change // Save the pictures into the arrays if(FB_CopyVGARect(INITIAL_X, INITIAL_Y, WIDTH_GRAB_VIEW, HEIGHT_GRAB_VIEW, (char HUGE *)PixelArray, -1, COPYDIR_FROMOFFSCREEN) == RET_ERROR){ error(4); return RET_ERROR; } printf("The FlushBus successfuly grab the field image and stored in the array.\n\n"); return RET_SUCCESS; } /***************************************************************************** * Following procedure only looks for the ball. ******************************************************************************/ void findBall(){ // counter int i,j; // store the pixel value of the edge of the object PointInt top, bottom, left, right; // TRUE if the ball is found. FALSE otherwise. boolean isBallFound = FALSE; // character indicating the color char color; // Search the entire image, top to bottom, left to right. for( i = INITIAL_Y; i < HEIGHT_GRAB_VIEW && isBallFound == FALSE; i = i++){ for( j = INITIAL_X; j < WIDTH_GRAB_VIEW && isBallFound == FALSE; j = j++){ //Look for the yellow ball if(i <= HEIGHT_GRAB_VIEW-6 && j <= WIDTH_GRAB_VIEW-6 && returnColor(PixelArray[i][j]) == YELLOW && returnColor(PixelArray[i+1][j]) == YELLOW && returnColor(PixelArray[i+2][j]) == YELLOW && returnColor(PixelArray[i-1][j]) == YELLOW && returnColor(PixelArray[i-2][j]) == YELLOW && returnColor(PixelArray[i][j+2]) == YELLOW && returnColor(PixelArray[i][j-2]) == YELLOW){ isBallFound = TRUE; color = YELLOW; //get a rough idea of the center of the ball //top will hold the i location of the first non-yellow pixel ABOVE the expected center //bottom will hold the i location of the first non-yellow pixel BELOW etc. etc. //1st round top.X = j; top.Y = i; bottom.X = j; bottom.Y = i; left.X = j; left.Y = i; right.X = j; right.Y = i; while(returnColor(PixelArray[--top.Y][top.X]) == color); while(returnColor(PixelArray[++bottom.Y][bottom.X]) == color); while(returnColor(PixelArray[left.Y][--left.X]) == color); while(returnColor(PixelArray[right.Y][++right.X]) == color); preBallPos.X = (left.X+right.X)/2; + 1; preBallPos.Y = (top.Y+bottom.Y)/2; findCenterOfMass(&preBallPos.X,&preBallPos.Y); //2nd round top.X = (int)preBallPos.X; top.Y = (int)preBallPos.Y; bottom.X = (int)preBallPos.X; bottom.Y = (int)preBallPos.Y; left.X = (int)preBallPos.X; left.Y = (int)preBallPos.Y; right.X = (int)preBallPos.X; right.Y = (int)preBallPos.Y; while(returnColor(PixelArray[--top.Y][top.X]) == color); while(returnColor(PixelArray[++bottom.Y][bottom.X]) == color);