For IEEE Robotics
Dependencies: Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos
Diff: ShapeDetect.cpp
- Revision:
- 0:1b64a0cedc5d
- Child:
- 3:b7b4780a7f6e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ShapeDetect.cpp Mon Mar 03 15:27:32 2014 +0000 @@ -0,0 +1,308 @@ +#include "mbed.h" +#include "ShapeDetect.h" + +LocalFileSystem local("local"); +DigitalOut myled(LED1); +Serial lrf(p9,p10); +extern Serial pc; + + +/* variables used */ +unsigned char image[120][160]; +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("Capture 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); + 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<10; i++){ + for(int j=0; j<160; j++){ + image[i][j] = 0; + + }// j + }// i + + //bottom 20 pixels + for(int i=109; 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<10; j++){ + image[i][j] = 0; + }// j + }// i + //right 20 pixels + for(int i=0; i<120; i++){ + for(int j=149; j<160; j++){ + image[i][j] = 0; + }// j + }// i + //clear bounds of detected blacks +} + +int shapeDetection_mass(void){ + + centerMass(&xcoord_val, &ycoord_val, &s_area_val); + if(s_area_val > 2800){ + pc.printf("\nSQUARE DETECTECD\n\r"); + return 3; + } else if (s_area_val < 2200){ + pc.printf("\nTRIANGLE DETECTECD\n\r"); + return 1; + }else{ + pc.printf("\nCIRCLE DETECTECD\n\r"); + return 2; + } + + } +int laserDistance(void){ + myled=1; + 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(); + myled=0; + //eat CR & ":" command prompt + do { + lrfchar=lrf.getc(); + } while (lrfchar != ':'); + return 1; +} + +