nav fixed
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Fork of theRobot by
ShapeDetect.cpp
- Committer:
- jjcarr2
- Date:
- 2014-04-11
- Revision:
- 22:072dabdf905a
- Parent:
- 20:55dcff40c5d9
File content as of revision 22:072dabdf905a:
#include "mbed.h" #include "ShapeDetect.h" LocalFileSystem local("local"); Serial lrf(p13,p14); //LaserRangeFinder Camera extern Serial pc; /* variables used */ //IMAGE PROCESSING unsigned char image[120][160]; //image array unsigned char pixel; char lrfchar=0; unsigned int s_pixel[8]; int num_edges = 0, i=0, j=0; int mm_range=0; int s_pixel_sum = 0; char min; char max; int xcoord_val; int ycoord_val; int s_area_val; /********************************************** * * Initializes the LRF * ***********************************************/ void lrf_baudCalibration(void){ wait(2.5); lrf.baud(115200); do { lrf.putc('U'); pc.putc('.'); wait(.2); if (lrf.readable()) lrfchar = lrf.getc(); } while (lrfchar != ':'); pc.printf("\n\r"); // clear out any extra characters - just in case while (lrf.readable()) { lrfchar = lrf.getc(); } } /********************************************** * Turns all pixels to a value and stores in * image array * * input arrayType_a: * 1 = BINARY ( 1 or 0 ) * 2 = GREYSCALE (greyscale value 0x00 to 0xFF) * ***********************************************/ void ImageToArray(int arrayType_a){ lrf.putc('G'); pc.printf("Capturing Image\n\r"); // Fill the data with values switch(arrayType_a){ case 1: for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ pixel = lrf.getc(); if (pixel < THRESHOLD){ image[i][j] = 1; } else {image[i][j] = 0;} }// end j for loop }// end i for loop break; case 2: for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ image[i][j] = lrf.getc(); }// j }// i break; default: for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ pixel = lrf.getc(); if (pixel < THRESHOLD){ image[i][j] = 1; } else {image[i][j] = 0;} }// j }// i }//switch } /*************************************************** * prints 160x120 image array to files (.txt & .dat) * * stores on the mbed locally * * input arrayType_f: * 1 = BINARY (1 or 0) * 2 = GREYSCALE (greyscale value 0x00 to 0xFF) * 3 = DECIMAL (greyscale value 0 - 255) * ****************************************************/ void printImageToFile(int arrayType_f){ // Write to local files FILE *fp = fopen("/local/image.dat", "w"); FILE *fp2 = fopen("/local/image.txt", "w"); for (int i=0; i<120; i++) { for(int j=0; j<160; j++){ switch (arrayType_f){ //BINARY OUTPUT case 1: fprintf(fp, "%d",image[i][j]); fprintf(fp2, "%d",image[i][j]); break; //GREYSCALE OUTPUT 00 to FF case 2: fprintf(fp, "%02X ",image[i][j]); fprintf(fp2, "%02X ",image[i][j]); break; //DECIMAL OUTPUT case 3: fprintf(fp, "%3d ",image[i][j]); fprintf(fp2, "%3d ",image[i][j]); break; default: fprintf(fp, "%02h",image[i][j]); fprintf(fp2, "%02h",image[i][j]); break; } } fprintf(fp, "\n"); fprintf(fp2, "\n"); } fclose(fp); } /********************************************** * edgeDetection * * outputs the number of edges detected * * ***********************************************/ int edgeDetection(void){ ImageToArray(BINARY); for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ s_pixel_sum = 0; //stores the value of 8 surrounding pixels s_pixel[0] = image[i-1][j-1]; s_pixel[1] = image[i-1][j]; s_pixel[2] = image[i-1][j+1]; s_pixel[3] = image[i][j-1]; s_pixel[4] = image[i][j+1]; s_pixel[5] = image[i+1][j-1]; s_pixel[6] = image[i+1][j]; s_pixel[7] = image[i+1][j+1]; //get the sum of the pixels for (int k=0; k<8; k++){ s_pixel_sum+=s_pixel[k]; } if ((s_pixel_sum < 5) && (s_pixel_sum > 2)) { image[i][j] = 1; } else { image[i][j] = 0; } }// for j for loop }// end i for loop //fill image array with buffer values /// DO EDGE CALCULATION HERE return num_edges; } // end function /********************************************** * center of mass coordinates * * outputs the coordinmates * ***********************************************/ void centerMass(int *xcoord, int *ycoord, int *s_area){ ImageToArray(BINARY); //printImageToFile(BINARY); clearBounds(); int count = 0; int sumi = 0; int sumj = 0; for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ if(image[i][j] == 1){ sumi += i; sumj += j; count++; } }// j }// i int x_2coord = sumj / count; int y_2coord = sumi / count; *xcoord = x_2coord; *ycoord = y_2coord; image[x_2coord][x_2coord] = 8; int s_2area = 0; for(int i=0; i<120; i++){ for(int j=0; j<160; j++){ if ( image[i][j] == 1 ){ s_2area++; } }// j }// i *s_area = s_2area; } /*********************************************** * * clears edges for more accurate area counting * * ************************************************/ void clearBounds(void){ //top 20 pixels for(int i=0; i<5; i++){ for(int j=0; j<160; j++){ image[i][j] = 0; }// j }// i //bottom 20 pixels for(int i=114; i<120; i++){ for(int j=0; j<160; j++){ image[i][j] = 0; }// j }// i //left 20 pixels for(int i=0; i<120; i++){ for(int j=0; j<20; j++){ image[i][j] = 0; }// j }// i //right 20 pixels for(int i=0; i<120; i++){ for(int j=139; j<160; j++){ image[i][j] = 0; }// j }// i //clear bounds of detected blacks } int shapeDetection(void) { centerMass(&xcoord_val, &ycoord_val, &s_area_val); pc.printf("\ncentriod calculated\n\r"); pc.printf("Center of Mass is at X: %d Y: %d\n\r", xcoord_val, ycoord_val, s_area_val); pc.printf("The area of the Mass is: %d\n\r", s_area_val); /* if( (s_area_val > SQUARE_AREA) ) { pc.printf("\nSQUARE DETECTECD\n\r"); return 1; } else if (s_area_val < TRIANGLE_AREA) { pc.printf("\nTRIANGLE DETECTECD\n\r"); return 2; } else { pc.printf("\nCIRCLE DETECTECD\n\r"); return 3; } */ return s_area_val; } int getDistance(void){ lrf.putc('B'); //Take Binary range reading // read in the four bytes for the range in mm (MSB first) mm_range=0; mm_range=lrf.getc(); mm_range=(mm_range<<8)|lrf.getc(); mm_range=(mm_range<<8)|lrf.getc(); mm_range=(mm_range<<8)|lrf.getc(); //eat CR & ":" command prompt do { lrfchar=lrf.getc(); } while (lrfchar != ':'); return mm_range; } int get_com_x(void) { return xcoord_val; } int get_com_y(void) { return ycoord_val; } int get_com_a(void) { return s_area_val; } int rigDetectionImgProc(void){ ImageToArray(GREYSCALE); int count1 = 0; int count2 = 1; //first rig for(int i=40; i<80; i++){ for(int j=0; j<60; j++){ if (image[i][j] < 128); count1++; }// j }// i //second rig for(int i=0; i<120; i++){ for(int j=139; j<160; j++){ if (image[i][j] < 128); count2++; }// j }// i if( (count1 < RIG_IP_THRESHOLD) && (count2 < RIG_IP_THRESHOLD) ){ //no fire detected on forst two rigs return 3; // CIRCLE RIG } else if( count1 > RIG_1_IP_THRESHOLD ){ return 1; //SQUARE RIG } else { return 2; //TRIANGLE RIG } }