Paolina Povolotskaya
TJPS
Fork of
Laser_Rangefinder
by 
jim hamblen
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