Paolina Povolotskaya
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
Diff: ShapeDetect.cpp
- Revision:
- 0:1b64a0cedc5d
- Child:
- 3:587441455259
--- /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;
+}
+
+