![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TJPS
Fork of Laser_Rangefinder by
Diff: LRF.c
- Revision:
- 1:d576753ebd76
--- /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