File content as of revision 1:d576753ebd76:

#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;
    
}