ShapeDetect.cpp

00001 #include "mbed.h"
00002 #include "ShapeDetect.h"
00003 
00004 LocalFileSystem local("local");   
00005 Serial lrf(p13,p14);                    //LaserRangeFinder Camera
00006 extern Serial pc;
00007 
00008 
00009 /* variables used */
00010 
00011 //IMAGE PROCESSING
00012 unsigned char image[120][160];          //image array             
00013 unsigned char pixel;                           
00014 
00015 char lrfchar=0;                                 
00016 unsigned int s_pixel[8];
00017 int num_edges = 0, i=0, j=0;
00018 int mm_range=0;
00019 
00020 int s_pixel_sum = 0;
00021 char min;
00022 char max;
00023 
00024 int xcoord_val;
00025 int ycoord_val; 
00026 int s_area_val;
00027 
00028 
00029 /**********************************************
00030 *    
00031 *   Initializes the LRF
00032 *   
00033 ***********************************************/
00034 
00035 void lrf_baudCalibration(void){
00036     wait(2.5);
00037     lrf.baud(115200);  
00038     do {
00039         lrf.putc('U');
00040         pc.putc('.');
00041         wait(.2);
00042         if (lrf.readable()) lrfchar = lrf.getc();
00043     } while (lrfchar != ':');
00044     pc.printf("\n\r");
00045     // clear out any extra characters - just in case
00046     while (lrf.readable()) {
00047         lrfchar = lrf.getc();
00048     }
00049 }
00050 
00051 
00052 
00053 /**********************************************
00054 *   Turns all pixels to a value and stores in 
00055 *          image array
00056 *   
00057 *    input arrayType_a:
00058 *           1 = BINARY  ( 1 or 0 ) 
00059 *           2 = GREYSCALE (greyscale value  0x00 to 0xFF)
00060 *   
00061 ***********************************************/
00062 void ImageToArray(int arrayType_a){
00063     lrf.putc('G');
00064     pc.printf("Capturing Image\n\r");
00065     // Fill the data with values
00066     switch(arrayType_a){
00067         
00068         case 1:
00069             for(int i=0; i<120; i++){
00070                  for(int j=0; j<160; j++){
00071                      pixel = lrf.getc();
00072                      if (pixel < THRESHOLD){
00073                       image[i][j] = 1;
00074                       } else {image[i][j] = 0;}
00075                     }// end j for loop
00076                  }// end i for loop
00077         break;
00078         
00079         case 2:
00080         for(int i=0; i<120; i++){
00081                  for(int j=0; j<160; j++){
00082                       image[i][j] = lrf.getc();
00083                       }// j
00084                  }// i
00085         break;
00086         
00087         default:
00088             for(int i=0; i<120; i++){
00089                  for(int j=0; j<160; j++){
00090                      pixel = lrf.getc();
00091                      if (pixel < THRESHOLD){
00092                       image[i][j] = 1;
00093                       } else {image[i][j] = 0;}
00094                     }// j
00095                  }// i
00096         }//switch
00097 }
00098     
00099     
00100 /***************************************************
00101 *   prints 160x120 image array to files (.txt & .dat)
00102 *   
00103 *   stores on the mbed locally
00104 *
00105 *   input arrayType_f:
00106 *           1 = BINARY  (1 or 0)
00107 *           2 = GREYSCALE (greyscale value  0x00 to 0xFF)
00108 *           3 = DECIMAL   (greyscale value 0 - 255)
00109 *
00110 ****************************************************/
00111 
00112 
00113 void printImageToFile(int arrayType_f){
00114  
00115     // Write to local files
00116     FILE *fp = fopen("/local/image.dat", "w");  
00117     FILE *fp2 = fopen("/local/image.txt", "w"); 
00118     
00119     for (int i=0; i<120; i++) {
00120         for(int j=0; j<160; j++){
00121                  switch (arrayType_f){
00122                      
00123                      //BINARY OUTPUT
00124                      case 1:
00125                         fprintf(fp, "%d",image[i][j]);
00126                         fprintf(fp2, "%d",image[i][j]);
00127                         break;
00128                      
00129                      //GREYSCALE OUTPUT  00 to FF
00130                      case 2:
00131                         fprintf(fp, "%02X ",image[i][j]); 
00132                         fprintf(fp2, "%02X ",image[i][j]); 
00133                         break;
00134                      
00135                      //DECIMAL OUTPUT   
00136                      case 3:
00137                         fprintf(fp, "%3d ",image[i][j]);
00138                         fprintf(fp2, "%3d ",image[i][j]);
00139                         break;
00140                     
00141                     default:
00142                         fprintf(fp, "%02h",image[i][j]);
00143                         fprintf(fp2, "%02h",image[i][j]);
00144                         break;
00145                  }
00146         }
00147         fprintf(fp, "\n");
00148         fprintf(fp2, "\n");
00149     }
00150     fclose(fp);
00151 }
00152 
00153    
00154 /**********************************************
00155 *   edgeDetection 
00156 *   
00157 *   outputs the number of edges detected 
00158 *
00159 *
00160 ***********************************************/
00161 int edgeDetection(void){  
00162 
00163     ImageToArray(BINARY);
00164 
00165     for(int i=0; i<120; i++){
00166                  for(int j=0; j<160; j++){
00167                      s_pixel_sum = 0;
00168                      //stores the value of 8 surrounding pixels
00169                      s_pixel[0] = image[i-1][j-1];
00170                      s_pixel[1] = image[i-1][j];
00171                      s_pixel[2] = image[i-1][j+1];
00172                      s_pixel[3] = image[i][j-1];
00173                      s_pixel[4] = image[i][j+1];
00174                      s_pixel[5] = image[i+1][j-1];
00175                      s_pixel[6] = image[i+1][j];
00176                      s_pixel[7] = image[i+1][j+1];
00177                      
00178                      //get the sum of the pixels
00179                      for (int k=0; k<8; k++){
00180                          s_pixel_sum+=s_pixel[k];
00181                         }
00182                         
00183                      if ((s_pixel_sum < 5) && (s_pixel_sum > 2)) {
00184                          image[i][j] = 1;
00185                          } else { 
00186                          image[i][j] = 0;
00187                          }
00188                      
00189                          
00190                      }// for j for loop
00191                  }// end i for loop
00192                  //fill image array with buffer values
00193                 
00194                     
00195                  
00196                  /// DO EDGE CALCULATION HERE
00197                  return num_edges;    
00198 } // end function
00199 
00200 
00201 
00202 /**********************************************
00203 *   center of mass coordinates
00204 *   
00205 *   outputs the coordinmates  
00206 *
00207 ***********************************************/
00208 
00209 void centerMass(int *xcoord, int *ycoord, int *s_area){
00210     ImageToArray(BINARY);
00211     //printImageToFile(BINARY);
00212     clearBounds();
00213     int count = 0;
00214     int sumi = 0;
00215     int sumj = 0;
00216     for(int i=0; i<120; i++){
00217                  for(int j=0; j<160; j++){
00218                      if(image[i][j] == 1){
00219                      sumi += i;
00220                      sumj += j;
00221                      count++;
00222                      }
00223                     }// j
00224                  }// i          
00225     
00226     int x_2coord = sumj / count;
00227     int y_2coord = sumi / count;
00228     *xcoord = x_2coord;
00229     *ycoord = y_2coord;
00230     image[x_2coord][x_2coord] = 8;
00231     
00232     int s_2area = 0;
00233     for(int i=0; i<120; i++){
00234                  for(int j=0; j<160; j++){
00235                      if ( image[i][j] == 1 ){
00236                         s_2area++;
00237                         } 
00238                      
00239                     }// j
00240                  }// i
00241     *s_area = s_2area;
00242 }
00243 /***********************************************
00244 *
00245 *   clears edges for more accurate area counting
00246 *
00247 *
00248 ************************************************/
00249 void clearBounds(void){
00250         //top 20 pixels
00251         for(int i=0; i<5; i++){
00252                  for(int j=0; j<160; j++){
00253                      image[i][j] = 0;
00254                         
00255                     }// j
00256                  }// i
00257                  
00258         //bottom 20 pixels         
00259         for(int i=114; i<120; i++){
00260                  for(int j=0; j<160; j++){
00261                      image[i][j] = 0;        
00262                     }// j
00263                  }// i
00264         //left 20 pixels         
00265         for(int i=0; i<120; i++){
00266                  for(int j=0; j<20; j++){
00267                      image[i][j] = 0;     
00268                     }// j
00269                  }// i
00270          //right 20 pixels         
00271         for(int i=0; i<120; i++){
00272                  for(int j=139; j<160; j++){
00273                      image[i][j] = 0;    
00274                     }// j
00275                  }// i
00276        //clear bounds of detected blacks           
00277 }   
00278 
00279 
00280 int shapeDetection(void)
00281 {
00282 
00283     centerMass(&xcoord_val, &ycoord_val, &s_area_val);
00284     pc.printf("\ncentriod calculated\n\r");
00285     pc.printf("Center of Mass is at X: %d    Y: %d\n\r", xcoord_val, ycoord_val, s_area_val);
00286     pc.printf("The area of the Mass is: %d\n\r", s_area_val);
00287     
00288     //if( (s_area_val > (SQUARE_AREA - AREA_TOLERANCE)) && (s_area_val < (SQUARE_AREA + AREA_TOLERANCE)) ) {
00289     if( (s_area_val > 3200) ) {
00290         pc.printf("\nSQUARE DETECTECD\n\r");
00291         return 1;
00292     } 
00293     //else if ((s_area_val > (TRIANGLE_AREA - AREA_TOLERANCE)) && (s_area_val < (TRIANGLE_AREA + AREA_TOLERANCE)) ) {
00294         else if (s_area_val < 2600) {
00295         pc.printf("\nTRIANGLE DETECTECD\n\r");
00296         return 2;
00297     } else {
00298         pc.printf("\nCIRCLE DETECTECD\n\r");
00299         return 3;
00300     }
00301 }
00302 
00303 
00304 int getDistance(void){
00305         lrf.putc('B'); //Take Binary range reading
00306         // read in the four bytes for the range in mm (MSB first)
00307         mm_range=0;
00308         mm_range=lrf.getc();
00309         mm_range=(mm_range<<8)|lrf.getc();
00310         mm_range=(mm_range<<8)|lrf.getc();
00311         mm_range=(mm_range<<8)|lrf.getc();
00312         //eat CR & ":" command prompt
00313         do {
00314             lrfchar=lrf.getc();
00315         } while (lrfchar != ':');
00316         return mm_range;
00317 }
00318 
00319 
00320 int get_com_x(void)
00321 {
00322     
00323   return xcoord_val;   
00324     }
00325 int get_com_y(void)
00326 {
00327     return ycoord_val; 
00328     }
00329     
00330 int get_com_a(void)
00331 {
00332     return s_area_val;
00333 }
00334     
00335     
00336 int rigDetectionImgProc(void){
00337     
00338     ImageToArray(GREYSCALE);
00339     int count1 = 0;
00340     int count2 = 1;         
00341         
00342         //first rig       
00343         for(int i=40; i<80; i++){
00344                  for(int j=0; j<60; j++){
00345                      if (image[i][j] < 128); 
00346                      count1++;
00347                     }// j
00348                  }// i
00349                  
00350          //second rig        
00351         for(int i=0; i<120; i++){
00352                  for(int j=139; j<160; j++){
00353                      if (image[i][j] < 128); 
00354                      count2++;   
00355                     }// j
00356                  }// i
00357                  
00358         if( (count1 < RIG_IP_THRESHOLD)  && (count2 < RIG_IP_THRESHOLD) ){
00359             //no fire detected on forst two rigs
00360             return 3; // CIRCLE RIG
00361             } else if(  count1 > RIG_1_IP_THRESHOLD ){
00362                 return 1; //SQUARE RIG
00363                 } else {
00364                     return 2; //TRIANGLE RIG
00365                     }
00366     
00367     }