Thomas Ashworth / theRobot

For IEEE Robotics

Dependencies:   Adafruit-16-Ch-PWM-Servo-Driver HCSR04 PID PololuQik2 QEI Sharp mbed-rtos

ShapeDetect.cpp@3:b7b4780a7f6e, 2014-03-13 (annotated)

Committer:
tashworth
Date:
Thu Mar 13 17:06:34 2014 +0000
Revision:
3:b7b4780a7f6e
Parent:
0:1b64a0cedc5d
Child:
6:75259c3306dd
3-13-14; - working servo normalized to degrees 0-180; - working shape detection with camera 11" from ground plane; - working area of shape calculation; - working center of mass; - writes binary, decimal, or hex .dat and.txt file of the image to local

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