![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TJPS
Fork of Laser_Rangefinder by
LRF.c
- Committer:
- Fairy_Paolina
- Date:
- 2014-02-24
- Revision:
- 1:d576753ebd76
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; }