Revision 1:d576753ebd76, committed 2014-02-24

Comitter:

Fairy_Paolina

Date:

Mon Feb 24 22:51:57 2014 +0000

Parent:

0:ccc26a94788d

Commit message:

LRF

Changed in this revision

diff -r ccc26a94788d -r d576753ebd76 LRF.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LRF.c	Mon Feb 24 22:51:57 2014 +0000
@@ -0,0 +1,311 @@
+#include "mbed.h"
+#include "LRF.h"
+
+LocalFileSystem fs("local");
+
+DigitalOut myled(LED1);
+Serial lrf(p9,p10);
+extern Serial pc;
+
+void LRFinit(void){
+   char lrfchar=0;
+   
+   //Delay for lrf power on startup
+    wait(2.5);
+    lrf.baud(115200);
+
+    // lrf autobaud setup
+    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();
+    }     
+}
+///////////////////////////////////////////////////////////////////////////////////////////////////
+int Verify(int* slope, int* s, int shape){
+    int i=0, num[4], count[4];
+    
+    count[0]=count[1]=0;
+    
+    // check if at least two of the 4 sides are parallel
+        slope[0]=num[0];
+        
+        for(i=0; i<s[0]; i++){// find two most occuring slopes
+            if(slope[i] == num[0])count[0]++;
+            else if(slope[i] == num[1]) count[1]++;
+            else if(count[0]<2){
+                slope[i]=num[0];
+                count[0]=0;
+            }
+            else if(count[1]<2){
+                slope[i]=num[1];
+                count[1]=0;
+            }
+        }
+        
+        slope[1]=num[2];    
+        for(i=s[1]; i<s[2]; i++){// find two most occuring slopes
+            if(slope[i] == num[2])count[2]++;
+            else if(slope[i] == num[3]) count[3]++;
+            else if(count[2]<2){
+                slope[i]=num[2];
+                count[2]=0;
+            }
+            else if(count[3]<2){
+                slope[i]=num[3];
+                count[3]=0;
+            }
+        }
+            
+       if((slope[1]==slope[3] || slope[1]==slope[2])&&(slope[0]==slope[2] || slope[0]==slope[3])&&(shape==4))return 1;
+       
+       slope[2]=num[0];
+       for(i=s[0]; i<s[1]; i++){// find two most occuring slopes
+            if(slope[i] == num[0])count[0]++;
+            else if(slope[i] == num[1]) count[1]++;
+            else if(count[0]<2){
+                slope[i]=num[0];
+                count[0]=0;
+            }
+            else if(count[1]<2){
+                slope[i]=num[1];
+                count[1]=0;
+            }
+        }
+        
+        slope[3]=num[2];    
+        for(i=s[2]; i<s[3]; i++){// find two most occuring slopes
+            if(slope[i] == num[2])count[2]++;
+            else if(slope[i] == num[3]) count[3]++;
+            else if(count[2]<2){
+                slope[i]=num[2];
+                count[2]=0;
+            }
+            else if(count[3]<2){
+                slope[i]=num[3];
+                count[3]=0;
+            }
+        }
+        
+       if((slope[1]==slope[3] || slope[1]==slope[2])&&(slope[0]==slope[2] || slope[0]==slope[3])&&(shape==4))return 1;
+                      
+            
+    //else if(shape == 3);
+    //else
+    
+    
+    return 0;
+}
+///////////////////////////////////////////////////////////////////////////////////////////////////
+int Shape(unsigned char** array){
+        int i, j, k=0, lastk, prev, sides=1, s[4];
+        unsigned char location[128*4][2]; //filled with x,y corrdinates             
+        int slope[128]; 
+        FILE* fp;
+             
+   fp = fopen("/local/file.xls", "wb");
+    if (fp == NULL) {
+        return -2;
+    }
+    // scan starts in bottom left hand corner of the fram x,y (0,0)
+    // scanning around the shape in a clockwise direction
+     
+    // check for where edges start and save locations (scan left to right) 
+    k=0;           
+    for(i=0; i<128; i++){
+                 for(j=0; j<160; j++){
+                          if(array[i][j] == 1){ // if BLACK 
+                                         location[k][1]= i; // y
+                                         location[k][0]= j; // x                                        
+                                         k++;
+                                         j=160;
+
+                          }}}
+    prev= location[k-1][0]; // save last j value
+    
+    // scan from top to bottom 
+    for(j=prev; j>160; j++){
+                for(i=128; i>0; i--){
+                           if(array[i][j] == 1){ // if BLACK 
+                                         location[k][1]= i; // y
+                                         location[k][0]= j; // x                                        
+                                         k++;
+                                         i=0;
+                          }}} 
+    
+    prev= location[k-1][1]; // save last i value
+    
+    // scan from right to left 
+    for(i=prev; i>0; i--){
+                for(j=159; j>0; j--){
+                           if(array[i][j] == 1){ // if BLACK 
+                                         location[k][1]= i; // y
+                                         location[k][0]= j; // x                                         
+                                         k++;
+                                         j=0;
+                          }}}
+                                                                         
+    prev= location[k-1][0]; // save last j value
+
+    // scan from bottom up 
+    for(j=prev; j>0; j--){
+                  for(i=0; i<127; i++){
+                          if(array[i][j] == 1){ // if BLACK 
+                                if((location[k][1] != location[0][1])&&(location[k][1] != location[1][1])&&(location[k][1] != location[2][1])&&(location[k][1] != location[3][1])){
+                                         if((location[k][0] != location[0][0])&&(location[k][0] != location[1][0])&&(location[k][0] != location[2][0])&&(location[k][0] != location[3][0])){
+                                                    location[k][1]= i; // y
+                                                    location[k][0]= j; // x                                      
+                                                    k++;
+                                                    i=127;
+                                }}
+                          }}}                                              
+    
+                           
+                                                          
+    // save the last k
+    lastk = k-1; 
+    for(i=0; i< lastk; i++)fprintf(fp, "%d\t%d\n", location[i][0], location[i][1]);
+    i=0;
+    k=0;
+    // find slopes of 8 segments
+    while(i< lastk){
+             
+             /*if((location[i+1][0] - location[i][0]) == 0) slope[k] =100;// arbitrarily choose a number to differentiante num/0 with 0
+             else slope[k]=(location[i+1][1] - location[i][1])/(location[i+1][0] - location[i][0]); 
+             k++; 
+             i++; */        
+             if((location[i+4][0] - location[i][0]) == 0) slope[k] =100;// arbitrarily choose a number to differentiante num/0 with 0
+             else slope[k]=(location[i+4][1] - location[i][1])/(location[i+4][0] - location[i][0]); 
+             k++; 
+             i+=4;    
+    }
+    lastk=k-1;
+     
+    
+    
+    i=j=0;
+    while(i<lastk){// check for slope changes
+        prev=slope[i];
+        printf("%d\n\r",slope[i]);
+        while(prev == slope[i]){
+                   i++;
+                   printf("%d \r\n",slope[i]);
+                   if((prev-slope[i])<=3 && (prev-slope[i])>=-3){
+                          if((prev>0) != (slope[i]<0)){
+                                       if((prev != 0) && (slope[i] != 0))prev=slope[i];  
+        }}}
+        
+        if((prev == slope[i+1]) || (slope[i+1] == slope[i-1]))i++;
+        else if((slope[i+1] == slope[i+2])&& i<lastk){    
+                s[sides]=i;
+                sides++; // if just one number then, skip over
+                printf("SIDES %d\n\r",sides);  
+        }
+        i++;
+    }
+    // compare last two slopes with beginning two slopes
+    if((slope[0]==slope[lastk]) || (slope[0]==slope[lastk-1]) || (slope[1]==slope[lastk]) || (slope[1]==slope[lastk-1]))sides--;
+    fclose(fp);
+    
+    // verify shapes
+    
+    if(!Verify(slope,s,sides))return 4; 
+    
+                                    
+    if(sides < 4) return 3; // Triange
+    else if(sides ==4) return 2; // square
+    else return 1; // circle
+
+}
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int CheckShapeInFrame(unsigned char ** Image){
+       int i ,j;
+       
+       // check that outer edges are white (0)
+       
+       //left edge
+       for(j=0; j<160; j++) if(Image[0][j] != 0) return 2; // move right 
+       //right edge
+       for(j=0; j<160; j++) if(Image[127][j] != 0) return 1; // move left
+       //top edge 
+       for(i=0; i<128; i++) if(Image[i][0] != 0) return 4; // move down  
+       //bottom edge 
+       for(i=0; i<128; i++) if(Image[i][159] != 0) return 3; // move up 
+       
+       return 0;             
+
+}   
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int GetShape(void){
+    char lrfchar=0, end[3];
+    int i, j, repeat=0, move=0;
+    unsigned char c;
+    unsigned char** Image;    
+ 
+    Image=(unsigned char**)malloc(128*sizeof(unsigned char*));
+    for(j=0; j<128; j++){
+        Image[j]=(unsigned char*)malloc(160*sizeof(unsigned char));
+    }
+    
+    //lrf.putc('E'); //Adjust lighting conditions
+    //wait(.2);
+    
+    do{ 
+   
+        // get grayscale image 
+        lrf.putc('G'); //Take Binary range reading
+        // read in each byte at a time filling in the Image
+        // 0= white
+        // 1= black
+        for(i=0; i<128; i++){
+            for(j=0; j<160; j++){
+                c=lrf.getc();
+                if(c>80)Image[i][j]= 0; // VERIFY THIS ! WAS 70
+                else Image[i][j] =1;      
+        }}    
+
+        // for error checking purposes
+        end[0]=lrf.getc();
+        end[1]=lrf.getc();
+        end[2]=lrf.getc();
+
+        move= CheckShapeInFrame(Image);
+        
+        if(move == 1) pc.printf("Move Left\r\n");
+        else if(move == 2) pc.printf("Move right\r\n");
+        else if(move == 3) pc.printf("Move up\r\n");
+        else if(move == 4) pc.printf("Move down\r\n");
+        else repeat = 1;
+        
+        if(!strcmp(end, "END"))pc.printf("Done!\r\n");
+        else repeat = 0; 
+        
+        
+        //eat CR & ":" command prompt
+        do {
+            if(lrf.readable())lrfchar=lrf.getc();
+        }while(lrfchar != ':'); 
+        
+        //i=Shape(Image);
+        
+        //if(i==4)repeat =0;
+    
+    }while(!repeat);    
+    
+
+    i=Shape(Image);
+    
+    for(j=0; j<128; j++){
+            free(Image[j]);
+    }
+    free(Image);
+    
+    return i;
+    
+}
\ No newline at end of file

diff -r ccc26a94788d -r d576753ebd76 LRF.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LRF.h	Mon Feb 24 22:51:57 2014 +0000
@@ -0,0 +1,10 @@
+#ifndef LRF_H_
+#define LRF_H_
+
+void LRFinit(void);
+int Shape(unsigned char y[128*160]);
+int GetShape(void);
+int CheckShapeInFrame(unsigned char ** Image);
+int Verify(int* slope, int* s, int shape);
+
+#endif
\ No newline at end of file

diff -r ccc26a94788d -r d576753ebd76 main.cpp
--- a/main.cpp	Sun Nov 06 02:08:57 2011 +0000
+++ b/main.cpp	Mon Feb 24 22:51:57 2014 +0000
@@ -1,50 +1,28 @@
 #include "mbed.h"
-//Demo program for the Parallax Laser Range Finder
-//
-DigitalOut myled(LED1);
+#include "string.h"
+#include "LRF.h"
+
 Serial pc(USBTX, USBRX);
-Serial lrf(p9,p10);
+extern DigitalOut myled;
+extern Serial lrf;
+
+#define TRIANGLE 3
+#define RECTANGLE 2
+#define CIRCLE 1
 
 int main() {
-    int mm_range=0;
-    char lrfchar=0;
-
-//Print prompt
-    pc.printf("\f\nLaser Range Finder demo program\n\r");
-//Delay for lrf power on startup
-    wait(2.5);
-    lrf.baud(115200);
+    int shape;
+    
+    LRFinit();
+    shape=GetShape();
+    pc.printf("Shape:\n");
+    
+    if(shape == TRIANGLE) pc.printf("Triangle\n");
+    else if(shape == RECTANGLE) pc.printf("Rectangle\n");
+    else pc.printf("Circle\n");
+    
 
-// lrf autobaud setup
-    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();
+
     }
 
 
-// Loop taking range readings forever
-    while (1) {
-        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 != ':');
-        //Display readings on PC Virtual Com port via USB cable
-        pc.printf("Range is %d mm\n\r",mm_range);
-    }
-}