ShapeDetect.cpp

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