Paolina Povolotskaya / Mbed 2 deprecated theRobot

added normalization and use of keyboard. I tested it and it worked

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver mbed

Fork of theRobot by Thomas Ashworth

ShapeDetect.cpp@4:116829a5ae3c, 2014-03-08 (annotated)

Committer:
Fairy_Paolina
Date:
Sat Mar 08 23:23:35 2014 +0000
Revision:
4:116829a5ae3c
Parent:
3:587441455259 
revision 3/8;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tashworth 0:1b64a0cedc5d 1 #include "mbed.h"
tashworth 0:1b64a0cedc5d 2 #include "ShapeDetect.h"
tashworth 0:1b64a0cedc5d 3
tashworth 0:1b64a0cedc5d 4 LocalFileSystem local("local");
tashworth 0:1b64a0cedc5d 5 DigitalOut myled(LED1);
tashworth 0:1b64a0cedc5d 6 Serial lrf(p9,p10);
tashworth 0:1b64a0cedc5d 7 extern Serial pc;
tashworth 0:1b64a0cedc5d 8
tashworth 0:1b64a0cedc5d 9
tashworth 0:1b64a0cedc5d 10 /* variables used */
tashworth 0:1b64a0cedc5d 11 unsigned char image[120][160];
tashworth 0:1b64a0cedc5d 12 unsigned char pixel;
tashworth 0:1b64a0cedc5d 13 char lrfchar=0;
tashworth 0:1b64a0cedc5d 14 unsigned int s_pixel[8];
tashworth 0:1b64a0cedc5d 15 int num_edges = 0, i=0, j=0;
tashworth 0:1b64a0cedc5d 16 int mm_range=0;
tashworth 0:1b64a0cedc5d 17
tashworth 0:1b64a0cedc5d 18 int s_pixel_sum = 0;
tashworth 0:1b64a0cedc5d 19 char min;
tashworth 0:1b64a0cedc5d 20 char max;
tashworth 0:1b64a0cedc5d 21
tashworth 0:1b64a0cedc5d 22 int xcoord_val;
tashworth 0:1b64a0cedc5d 23 int ycoord_val;
tashworth 0:1b64a0cedc5d 24 int s_area_val;
tashworth 0:1b64a0cedc5d 25
tashworth 0:1b64a0cedc5d 26
tashworth 0:1b64a0cedc5d 27 /**********************************************
tashworth 0:1b64a0cedc5d 28 *
tashworth 0:1b64a0cedc5d 29 * Initializes the LRF
tashworth 0:1b64a0cedc5d 30 *
tashworth 0:1b64a0cedc5d 31 ***********************************************/
tashworth 0:1b64a0cedc5d 32
tashworth 0:1b64a0cedc5d 33 void lrf_baudCalibration(void){
tashworth 0:1b64a0cedc5d 34 wait(2.5);
tashworth 0:1b64a0cedc5d 35 lrf.baud(115200);
tashworth 0:1b64a0cedc5d 36 do {
tashworth 0:1b64a0cedc5d 37 lrf.putc('U');
tashworth 0:1b64a0cedc5d 38 pc.putc('.');
tashworth 0:1b64a0cedc5d 39 wait(.2);
tashworth 0:1b64a0cedc5d 40 if (lrf.readable()) lrfchar = lrf.getc();
tashworth 0:1b64a0cedc5d 41 } while (lrfchar != ':');
tashworth 0:1b64a0cedc5d 42 pc.printf("\n\r");
tashworth 0:1b64a0cedc5d 43 // clear out any extra characters - just in case
tashworth 0:1b64a0cedc5d 44 while (lrf.readable()) {
tashworth 0:1b64a0cedc5d 45 lrfchar = lrf.getc();
tashworth 0:1b64a0cedc5d 46 }
tashworth 0:1b64a0cedc5d 47 }
tashworth 0:1b64a0cedc5d 48
tashworth 0:1b64a0cedc5d 49
tashworth 0:1b64a0cedc5d 50
tashworth 0:1b64a0cedc5d 51 /**********************************************
tashworth 0:1b64a0cedc5d 52 * Turns all pixels to a value and stores in
tashworth 0:1b64a0cedc5d 53 * image array
tashworth 0:1b64a0cedc5d 54 *
tashworth 0:1b64a0cedc5d 55 * input arrayType_a:
tashworth 0:1b64a0cedc5d 56 * 1 = BINARY ( 1 or 0 )
tashworth 0:1b64a0cedc5d 57 * 2 = GREYSCALE (greyscale value 0x00 to 0xFF)
tashworth 0:1b64a0cedc5d 58 *
tashworth 0:1b64a0cedc5d 59 ***********************************************/
tashworth 0:1b64a0cedc5d 60 void ImageToArray(int arrayType_a){
tashworth 0:1b64a0cedc5d 61 lrf.putc('G');
tashworth 0:1b64a0cedc5d 62 pc.printf("Capture Image\n\r");
tashworth 0:1b64a0cedc5d 63 // Fill the data with values
tashworth 0:1b64a0cedc5d 64 switch(arrayType_a){
tashworth 0:1b64a0cedc5d 65
tashworth 0:1b64a0cedc5d 66 case 1:
tashworth 0:1b64a0cedc5d 67 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 68 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 69 pixel = lrf.getc();
tashworth 0:1b64a0cedc5d 70 if (pixel < THRESHOLD){
tashworth 0:1b64a0cedc5d 71 image[i][j] = 1;
tashworth 0:1b64a0cedc5d 72 } else {image[i][j] = 0;}
tashworth 0:1b64a0cedc5d 73 }// end j for loop
tashworth 0:1b64a0cedc5d 74 }// end i for loop
tashworth 0:1b64a0cedc5d 75 break;
tashworth 0:1b64a0cedc5d 76
tashworth 0:1b64a0cedc5d 77 case 2:
tashworth 0:1b64a0cedc5d 78 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 79 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 80 image[i][j] = lrf.getc();
tashworth 0:1b64a0cedc5d 81 }// j
tashworth 0:1b64a0cedc5d 82 }// i
tashworth 0:1b64a0cedc5d 83 break;
tashworth 0:1b64a0cedc5d 84
tashworth 0:1b64a0cedc5d 85 default:
tashworth 0:1b64a0cedc5d 86 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 87 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 88 pixel = lrf.getc();
tashworth 0:1b64a0cedc5d 89 if (pixel < THRESHOLD){
tashworth 0:1b64a0cedc5d 90 image[i][j] = 1;
tashworth 0:1b64a0cedc5d 91 } else {image[i][j] = 0;}
tashworth 0:1b64a0cedc5d 92 }// j
tashworth 0:1b64a0cedc5d 93 }// i
tashworth 0:1b64a0cedc5d 94 }//switch
tashworth 0:1b64a0cedc5d 95 }
tashworth 0:1b64a0cedc5d 96
tashworth 0:1b64a0cedc5d 97
tashworth 0:1b64a0cedc5d 98 /***************************************************
tashworth 0:1b64a0cedc5d 99 * prints 160x120 image array to files (.txt & .dat)
tashworth 0:1b64a0cedc5d 100 *
tashworth 0:1b64a0cedc5d 101 * stores on the mbed locally
tashworth 0:1b64a0cedc5d 102 *
tashworth 0:1b64a0cedc5d 103 * input arrayType_f:
tashworth 0:1b64a0cedc5d 104 * 1 = BINARY (1 or 0)
tashworth 0:1b64a0cedc5d 105 * 2 = GREYSCALE (greyscale value 0x00 to 0xFF)
tashworth 0:1b64a0cedc5d 106 * 3 = DECIMAL (greyscale value 0 - 255)
tashworth 0:1b64a0cedc5d 107 *
tashworth 0:1b64a0cedc5d 108 ****************************************************/
tashworth 0:1b64a0cedc5d 109
tashworth 0:1b64a0cedc5d 110
tashworth 0:1b64a0cedc5d 111 void printImageToFile(int arrayType_f){
tashworth 0:1b64a0cedc5d 112
tashworth 0:1b64a0cedc5d 113 // Write to local files
tashworth 0:1b64a0cedc5d 114 FILE *fp = fopen("/local/image.dat", "w");
tashworth 0:1b64a0cedc5d 115 FILE *fp2 = fopen("/local/image.txt", "w");
tashworth 0:1b64a0cedc5d 116
tashworth 0:1b64a0cedc5d 117 for (int i=0; i<120; i++) {
tashworth 0:1b64a0cedc5d 118 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 119 switch (arrayType_f){
tashworth 0:1b64a0cedc5d 120
tashworth 0:1b64a0cedc5d 121 //BINARY OUTPUT
tashworth 0:1b64a0cedc5d 122 case 1:
tashworth 0:1b64a0cedc5d 123 fprintf(fp, "%d",image[i][j]);
tashworth 0:1b64a0cedc5d 124 fprintf(fp2, "%d",image[i][j]);
tashworth 0:1b64a0cedc5d 125 break;
tashworth 0:1b64a0cedc5d 126
tashworth 0:1b64a0cedc5d 127 //GREYSCALE OUTPUT 00 to FF
tashworth 0:1b64a0cedc5d 128 case 2:
tashworth 0:1b64a0cedc5d 129 fprintf(fp, "%02X ",image[i][j]);
tashworth 0:1b64a0cedc5d 130 fprintf(fp2, "%02X ",image[i][j]);
tashworth 0:1b64a0cedc5d 131 break;
tashworth 0:1b64a0cedc5d 132
tashworth 0:1b64a0cedc5d 133 //DECIMAL OUTPUT
tashworth 0:1b64a0cedc5d 134 case 3:
tashworth 0:1b64a0cedc5d 135 fprintf(fp, "%3d ",image[i][j]);
tashworth 0:1b64a0cedc5d 136 fprintf(fp2, "%3d ",image[i][j]);
tashworth 0:1b64a0cedc5d 137 break;
tashworth 0:1b64a0cedc5d 138
tashworth 0:1b64a0cedc5d 139 default:
tashworth 0:1b64a0cedc5d 140 fprintf(fp, "%02h",image[i][j]);
Fairy_Paolina 3:587441455259 141 + fprintf(fp2, "%02h",image[i][j]);
tashworth 0:1b64a0cedc5d 142 break;
tashworth 0:1b64a0cedc5d 143 }
tashworth 0:1b64a0cedc5d 144 }
tashworth 0:1b64a0cedc5d 145 fprintf(fp, "\n");
tashworth 0:1b64a0cedc5d 146 fprintf(fp2, "\n");
tashworth 0:1b64a0cedc5d 147 }
tashworth 0:1b64a0cedc5d 148 fclose(fp);
tashworth 0:1b64a0cedc5d 149 }
tashworth 0:1b64a0cedc5d 150
tashworth 0:1b64a0cedc5d 151
tashworth 0:1b64a0cedc5d 152 /**********************************************
tashworth 0:1b64a0cedc5d 153 * edgeDetection
tashworth 0:1b64a0cedc5d 154 *
tashworth 0:1b64a0cedc5d 155 * outputs the number of edges detected
tashworth 0:1b64a0cedc5d 156 *
tashworth 0:1b64a0cedc5d 157 *
tashworth 0:1b64a0cedc5d 158 ***********************************************/
tashworth 0:1b64a0cedc5d 159 int edgeDetection(void){
tashworth 0:1b64a0cedc5d 160
tashworth 0:1b64a0cedc5d 161 ImageToArray(BINARY);
tashworth 0:1b64a0cedc5d 162
tashworth 0:1b64a0cedc5d 163 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 164 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 165 s_pixel_sum = 0;
tashworth 0:1b64a0cedc5d 166 //stores the value of 8 surrounding pixels
tashworth 0:1b64a0cedc5d 167 s_pixel[0] = image[i-1][j-1];
tashworth 0:1b64a0cedc5d 168 s_pixel[1] = image[i-1][j];
tashworth 0:1b64a0cedc5d 169 s_pixel[2] = image[i-1][j+1];
tashworth 0:1b64a0cedc5d 170 s_pixel[3] = image[i][j-1];
tashworth 0:1b64a0cedc5d 171 s_pixel[4] = image[i][j+1];
tashworth 0:1b64a0cedc5d 172 s_pixel[5] = image[i+1][j-1];
tashworth 0:1b64a0cedc5d 173 s_pixel[6] = image[i+1][j];
tashworth 0:1b64a0cedc5d 174 s_pixel[7] = image[i+1][j+1];
tashworth 0:1b64a0cedc5d 175
tashworth 0:1b64a0cedc5d 176 //get the sum of the pixels
tashworth 0:1b64a0cedc5d 177 for (int k=0; k<8; k++){
tashworth 0:1b64a0cedc5d 178 s_pixel_sum+=s_pixel[k];
tashworth 0:1b64a0cedc5d 179 }
tashworth 0:1b64a0cedc5d 180
tashworth 0:1b64a0cedc5d 181 if ((s_pixel_sum < 5) && (s_pixel_sum > 2)) {
tashworth 0:1b64a0cedc5d 182 image[i][j] = 1;
tashworth 0:1b64a0cedc5d 183 } else {
tashworth 0:1b64a0cedc5d 184 image[i][j] = 0;
tashworth 0:1b64a0cedc5d 185 }
tashworth 0:1b64a0cedc5d 186
tashworth 0:1b64a0cedc5d 187
tashworth 0:1b64a0cedc5d 188 }// for j for loop
tashworth 0:1b64a0cedc5d 189 }// end i for loop
tashworth 0:1b64a0cedc5d 190 //fill image array with buffer values
tashworth 0:1b64a0cedc5d 191
tashworth 0:1b64a0cedc5d 192
tashworth 0:1b64a0cedc5d 193
tashworth 0:1b64a0cedc5d 194 /// DO EDGE CALCULATION HERE
tashworth 0:1b64a0cedc5d 195 return num_edges;
tashworth 0:1b64a0cedc5d 196 } // end function
tashworth 0:1b64a0cedc5d 197
tashworth 0:1b64a0cedc5d 198
tashworth 0:1b64a0cedc5d 199
tashworth 0:1b64a0cedc5d 200 /**********************************************
tashworth 0:1b64a0cedc5d 201 * center of mass coordinates
tashworth 0:1b64a0cedc5d 202 *
tashworth 0:1b64a0cedc5d 203 * outputs the coordinmates
tashworth 0:1b64a0cedc5d 204 *
tashworth 0:1b64a0cedc5d 205 ***********************************************/
tashworth 0:1b64a0cedc5d 206
tashworth 0:1b64a0cedc5d 207 void centerMass(int *xcoord, int *ycoord, int *s_area){
tashworth 0:1b64a0cedc5d 208 ImageToArray(BINARY);
tashworth 0:1b64a0cedc5d 209 clearBounds();
tashworth 0:1b64a0cedc5d 210 int count = 0;
tashworth 0:1b64a0cedc5d 211 int sumi = 0;
tashworth 0:1b64a0cedc5d 212 int sumj = 0;
tashworth 0:1b64a0cedc5d 213 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 214 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 215 if(image[i][j] == 1){
tashworth 0:1b64a0cedc5d 216 sumi += i;
tashworth 0:1b64a0cedc5d 217 sumj += j;
tashworth 0:1b64a0cedc5d 218 count++;
tashworth 0:1b64a0cedc5d 219 }
tashworth 0:1b64a0cedc5d 220 }// j
tashworth 0:1b64a0cedc5d 221 }// i
tashworth 0:1b64a0cedc5d 222
tashworth 0:1b64a0cedc5d 223 int x_2coord = sumj / count;
tashworth 0:1b64a0cedc5d 224 int y_2coord = sumi / count;
tashworth 0:1b64a0cedc5d 225 *xcoord = x_2coord;
tashworth 0:1b64a0cedc5d 226 *ycoord = y_2coord;
tashworth 0:1b64a0cedc5d 227 image[x_2coord][x_2coord] = 8;
tashworth 0:1b64a0cedc5d 228
tashworth 0:1b64a0cedc5d 229 int s_2area = 0;
tashworth 0:1b64a0cedc5d 230 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 231 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 232 if ( image[i][j] == 1 ){
tashworth 0:1b64a0cedc5d 233 s_2area++;
tashworth 0:1b64a0cedc5d 234 }
tashworth 0:1b64a0cedc5d 235
tashworth 0:1b64a0cedc5d 236 }// j
tashworth 0:1b64a0cedc5d 237 }// i
tashworth 0:1b64a0cedc5d 238 *s_area = s_2area;
tashworth 0:1b64a0cedc5d 239 }
tashworth 0:1b64a0cedc5d 240 /***********************************************
tashworth 0:1b64a0cedc5d 241 *
tashworth 0:1b64a0cedc5d 242 * clears edges for more accurate area counting
tashworth 0:1b64a0cedc5d 243 *
tashworth 0:1b64a0cedc5d 244 *
tashworth 0:1b64a0cedc5d 245 ************************************************/
tashworth 0:1b64a0cedc5d 246 void clearBounds(void){
tashworth 0:1b64a0cedc5d 247 //top 20 pixels
tashworth 0:1b64a0cedc5d 248 for(int i=0; i<10; i++){
tashworth 0:1b64a0cedc5d 249 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 250 image[i][j] = 0;
tashworth 0:1b64a0cedc5d 251
tashworth 0:1b64a0cedc5d 252 }// j
tashworth 0:1b64a0cedc5d 253 }// i
tashworth 0:1b64a0cedc5d 254
tashworth 0:1b64a0cedc5d 255 //bottom 20 pixels
tashworth 0:1b64a0cedc5d 256 for(int i=109; i<120; i++){
tashworth 0:1b64a0cedc5d 257 for(int j=0; j<160; j++){
tashworth 0:1b64a0cedc5d 258 image[i][j] = 0;
tashworth 0:1b64a0cedc5d 259 }// j
tashworth 0:1b64a0cedc5d 260 }// i
tashworth 0:1b64a0cedc5d 261 //left 20 pixels
tashworth 0:1b64a0cedc5d 262 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 263 for(int j=0; j<10; j++){
tashworth 0:1b64a0cedc5d 264 image[i][j] = 0;
tashworth 0:1b64a0cedc5d 265 }// j
tashworth 0:1b64a0cedc5d 266 }// i
tashworth 0:1b64a0cedc5d 267 //right 20 pixels
tashworth 0:1b64a0cedc5d 268 for(int i=0; i<120; i++){
tashworth 0:1b64a0cedc5d 269 for(int j=149; j<160; j++){
tashworth 0:1b64a0cedc5d 270 image[i][j] = 0;
tashworth 0:1b64a0cedc5d 271 }// j
tashworth 0:1b64a0cedc5d 272 }// i
tashworth 0:1b64a0cedc5d 273 //clear bounds of detected blacks
tashworth 0:1b64a0cedc5d 274 }
tashworth 0:1b64a0cedc5d 275
tashworth 0:1b64a0cedc5d 276 int shapeDetection_mass(void){
tashworth 0:1b64a0cedc5d 277
tashworth 0:1b64a0cedc5d 278 centerMass(&xcoord_val, &ycoord_val, &s_area_val);
tashworth 0:1b64a0cedc5d 279 if(s_area_val > 2800){
tashworth 0:1b64a0cedc5d 280 pc.printf("\nSQUARE DETECTECD\n\r");
tashworth 0:1b64a0cedc5d 281 return 3;
tashworth 0:1b64a0cedc5d 282 } else if (s_area_val < 2200){
tashworth 0:1b64a0cedc5d 283 pc.printf("\nTRIANGLE DETECTECD\n\r");
tashworth 0:1b64a0cedc5d 284 return 1;
tashworth 0:1b64a0cedc5d 285 }else{
tashworth 0:1b64a0cedc5d 286 pc.printf("\nCIRCLE DETECTECD\n\r");
tashworth 0:1b64a0cedc5d 287 return 2;
tashworth 0:1b64a0cedc5d 288 }
tashworth 0:1b64a0cedc5d 289
tashworth 0:1b64a0cedc5d 290 }
tashworth 0:1b64a0cedc5d 291 int laserDistance(void){
tashworth 0:1b64a0cedc5d 292 myled=1;
tashworth 0:1b64a0cedc5d 293 lrf.putc('B'); //Take Binary range reading
tashworth 0:1b64a0cedc5d 294 // read in the four bytes for the range in mm (MSB first)
tashworth 0:1b64a0cedc5d 295 mm_range=0;
tashworth 0:1b64a0cedc5d 296 mm_range=lrf.getc();
tashworth 0:1b64a0cedc5d 297 mm_range=(mm_range<<8)|lrf.getc();
tashworth 0:1b64a0cedc5d 298 mm_range=(mm_range<<8)|lrf.getc();
tashworth 0:1b64a0cedc5d 299 mm_range=(mm_range<<8)|lrf.getc();
tashworth 0:1b64a0cedc5d 300 myled=0;
tashworth 0:1b64a0cedc5d 301 //eat CR & ":" command prompt
tashworth 0:1b64a0cedc5d 302 do {
tashworth 0:1b64a0cedc5d 303 lrfchar=lrf.getc();
tashworth 0:1b64a0cedc5d 304 } while (lrfchar != ':');
tashworth 0:1b64a0cedc5d 305 return 1;
tashworth 0:1b64a0cedc5d 306 }
tashworth 0:1b64a0cedc5d 307
tashworth 0:1b64a0cedc5d 308