![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
TJPS
Fork of Laser_Rangefinder by
LRF.c@1:d576753ebd76, 2014-02-24 (annotated)
- Committer:
- Fairy_Paolina
- Date:
- Mon Feb 24 22:51:57 2014 +0000
- Revision:
- 1:d576753ebd76
LRF
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Fairy_Paolina | 1:d576753ebd76 | 1 | #include "mbed.h" |
Fairy_Paolina | 1:d576753ebd76 | 2 | #include "LRF.h" |
Fairy_Paolina | 1:d576753ebd76 | 3 | |
Fairy_Paolina | 1:d576753ebd76 | 4 | LocalFileSystem fs("local"); |
Fairy_Paolina | 1:d576753ebd76 | 5 | |
Fairy_Paolina | 1:d576753ebd76 | 6 | DigitalOut myled(LED1); |
Fairy_Paolina | 1:d576753ebd76 | 7 | Serial lrf(p9,p10); |
Fairy_Paolina | 1:d576753ebd76 | 8 | extern Serial pc; |
Fairy_Paolina | 1:d576753ebd76 | 9 | |
Fairy_Paolina | 1:d576753ebd76 | 10 | void LRFinit(void){ |
Fairy_Paolina | 1:d576753ebd76 | 11 | char lrfchar=0; |
Fairy_Paolina | 1:d576753ebd76 | 12 | |
Fairy_Paolina | 1:d576753ebd76 | 13 | //Delay for lrf power on startup |
Fairy_Paolina | 1:d576753ebd76 | 14 | wait(2.5); |
Fairy_Paolina | 1:d576753ebd76 | 15 | lrf.baud(115200); |
Fairy_Paolina | 1:d576753ebd76 | 16 | |
Fairy_Paolina | 1:d576753ebd76 | 17 | // lrf autobaud setup |
Fairy_Paolina | 1:d576753ebd76 | 18 | do { |
Fairy_Paolina | 1:d576753ebd76 | 19 | lrf.putc('U'); |
Fairy_Paolina | 1:d576753ebd76 | 20 | pc.putc('.'); |
Fairy_Paolina | 1:d576753ebd76 | 21 | wait(.2); |
Fairy_Paolina | 1:d576753ebd76 | 22 | if (lrf.readable()) lrfchar = lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 23 | } while (lrfchar != ':'); |
Fairy_Paolina | 1:d576753ebd76 | 24 | pc.printf("\n\r"); |
Fairy_Paolina | 1:d576753ebd76 | 25 | // clear out any extra characters - just in case |
Fairy_Paolina | 1:d576753ebd76 | 26 | while (lrf.readable()) { |
Fairy_Paolina | 1:d576753ebd76 | 27 | lrfchar = lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 28 | } |
Fairy_Paolina | 1:d576753ebd76 | 29 | } |
Fairy_Paolina | 1:d576753ebd76 | 30 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
Fairy_Paolina | 1:d576753ebd76 | 31 | int Verify(int* slope, int* s, int shape){ |
Fairy_Paolina | 1:d576753ebd76 | 32 | int i=0, num[4], count[4]; |
Fairy_Paolina | 1:d576753ebd76 | 33 | |
Fairy_Paolina | 1:d576753ebd76 | 34 | count[0]=count[1]=0; |
Fairy_Paolina | 1:d576753ebd76 | 35 | |
Fairy_Paolina | 1:d576753ebd76 | 36 | // check if at least two of the 4 sides are parallel |
Fairy_Paolina | 1:d576753ebd76 | 37 | slope[0]=num[0]; |
Fairy_Paolina | 1:d576753ebd76 | 38 | |
Fairy_Paolina | 1:d576753ebd76 | 39 | for(i=0; i<s[0]; i++){// find two most occuring slopes |
Fairy_Paolina | 1:d576753ebd76 | 40 | if(slope[i] == num[0])count[0]++; |
Fairy_Paolina | 1:d576753ebd76 | 41 | else if(slope[i] == num[1]) count[1]++; |
Fairy_Paolina | 1:d576753ebd76 | 42 | else if(count[0]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 43 | slope[i]=num[0]; |
Fairy_Paolina | 1:d576753ebd76 | 44 | count[0]=0; |
Fairy_Paolina | 1:d576753ebd76 | 45 | } |
Fairy_Paolina | 1:d576753ebd76 | 46 | else if(count[1]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 47 | slope[i]=num[1]; |
Fairy_Paolina | 1:d576753ebd76 | 48 | count[1]=0; |
Fairy_Paolina | 1:d576753ebd76 | 49 | } |
Fairy_Paolina | 1:d576753ebd76 | 50 | } |
Fairy_Paolina | 1:d576753ebd76 | 51 | |
Fairy_Paolina | 1:d576753ebd76 | 52 | slope[1]=num[2]; |
Fairy_Paolina | 1:d576753ebd76 | 53 | for(i=s[1]; i<s[2]; i++){// find two most occuring slopes |
Fairy_Paolina | 1:d576753ebd76 | 54 | if(slope[i] == num[2])count[2]++; |
Fairy_Paolina | 1:d576753ebd76 | 55 | else if(slope[i] == num[3]) count[3]++; |
Fairy_Paolina | 1:d576753ebd76 | 56 | else if(count[2]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 57 | slope[i]=num[2]; |
Fairy_Paolina | 1:d576753ebd76 | 58 | count[2]=0; |
Fairy_Paolina | 1:d576753ebd76 | 59 | } |
Fairy_Paolina | 1:d576753ebd76 | 60 | else if(count[3]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 61 | slope[i]=num[3]; |
Fairy_Paolina | 1:d576753ebd76 | 62 | count[3]=0; |
Fairy_Paolina | 1:d576753ebd76 | 63 | } |
Fairy_Paolina | 1:d576753ebd76 | 64 | } |
Fairy_Paolina | 1:d576753ebd76 | 65 | |
Fairy_Paolina | 1:d576753ebd76 | 66 | if((slope[1]==slope[3] || slope[1]==slope[2])&&(slope[0]==slope[2] || slope[0]==slope[3])&&(shape==4))return 1; |
Fairy_Paolina | 1:d576753ebd76 | 67 | |
Fairy_Paolina | 1:d576753ebd76 | 68 | slope[2]=num[0]; |
Fairy_Paolina | 1:d576753ebd76 | 69 | for(i=s[0]; i<s[1]; i++){// find two most occuring slopes |
Fairy_Paolina | 1:d576753ebd76 | 70 | if(slope[i] == num[0])count[0]++; |
Fairy_Paolina | 1:d576753ebd76 | 71 | else if(slope[i] == num[1]) count[1]++; |
Fairy_Paolina | 1:d576753ebd76 | 72 | else if(count[0]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 73 | slope[i]=num[0]; |
Fairy_Paolina | 1:d576753ebd76 | 74 | count[0]=0; |
Fairy_Paolina | 1:d576753ebd76 | 75 | } |
Fairy_Paolina | 1:d576753ebd76 | 76 | else if(count[1]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 77 | slope[i]=num[1]; |
Fairy_Paolina | 1:d576753ebd76 | 78 | count[1]=0; |
Fairy_Paolina | 1:d576753ebd76 | 79 | } |
Fairy_Paolina | 1:d576753ebd76 | 80 | } |
Fairy_Paolina | 1:d576753ebd76 | 81 | |
Fairy_Paolina | 1:d576753ebd76 | 82 | slope[3]=num[2]; |
Fairy_Paolina | 1:d576753ebd76 | 83 | for(i=s[2]; i<s[3]; i++){// find two most occuring slopes |
Fairy_Paolina | 1:d576753ebd76 | 84 | if(slope[i] == num[2])count[2]++; |
Fairy_Paolina | 1:d576753ebd76 | 85 | else if(slope[i] == num[3]) count[3]++; |
Fairy_Paolina | 1:d576753ebd76 | 86 | else if(count[2]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 87 | slope[i]=num[2]; |
Fairy_Paolina | 1:d576753ebd76 | 88 | count[2]=0; |
Fairy_Paolina | 1:d576753ebd76 | 89 | } |
Fairy_Paolina | 1:d576753ebd76 | 90 | else if(count[3]<2){ |
Fairy_Paolina | 1:d576753ebd76 | 91 | slope[i]=num[3]; |
Fairy_Paolina | 1:d576753ebd76 | 92 | count[3]=0; |
Fairy_Paolina | 1:d576753ebd76 | 93 | } |
Fairy_Paolina | 1:d576753ebd76 | 94 | } |
Fairy_Paolina | 1:d576753ebd76 | 95 | |
Fairy_Paolina | 1:d576753ebd76 | 96 | if((slope[1]==slope[3] || slope[1]==slope[2])&&(slope[0]==slope[2] || slope[0]==slope[3])&&(shape==4))return 1; |
Fairy_Paolina | 1:d576753ebd76 | 97 | |
Fairy_Paolina | 1:d576753ebd76 | 98 | |
Fairy_Paolina | 1:d576753ebd76 | 99 | //else if(shape == 3); |
Fairy_Paolina | 1:d576753ebd76 | 100 | //else |
Fairy_Paolina | 1:d576753ebd76 | 101 | |
Fairy_Paolina | 1:d576753ebd76 | 102 | |
Fairy_Paolina | 1:d576753ebd76 | 103 | return 0; |
Fairy_Paolina | 1:d576753ebd76 | 104 | } |
Fairy_Paolina | 1:d576753ebd76 | 105 | /////////////////////////////////////////////////////////////////////////////////////////////////// |
Fairy_Paolina | 1:d576753ebd76 | 106 | int Shape(unsigned char** array){ |
Fairy_Paolina | 1:d576753ebd76 | 107 | int i, j, k=0, lastk, prev, sides=1, s[4]; |
Fairy_Paolina | 1:d576753ebd76 | 108 | unsigned char location[128*4][2]; //filled with x,y corrdinates |
Fairy_Paolina | 1:d576753ebd76 | 109 | int slope[128]; |
Fairy_Paolina | 1:d576753ebd76 | 110 | FILE* fp; |
Fairy_Paolina | 1:d576753ebd76 | 111 | |
Fairy_Paolina | 1:d576753ebd76 | 112 | fp = fopen("/local/file.xls", "wb"); |
Fairy_Paolina | 1:d576753ebd76 | 113 | if (fp == NULL) { |
Fairy_Paolina | 1:d576753ebd76 | 114 | return -2; |
Fairy_Paolina | 1:d576753ebd76 | 115 | } |
Fairy_Paolina | 1:d576753ebd76 | 116 | // scan starts in bottom left hand corner of the fram x,y (0,0) |
Fairy_Paolina | 1:d576753ebd76 | 117 | // scanning around the shape in a clockwise direction |
Fairy_Paolina | 1:d576753ebd76 | 118 | |
Fairy_Paolina | 1:d576753ebd76 | 119 | // check for where edges start and save locations (scan left to right) |
Fairy_Paolina | 1:d576753ebd76 | 120 | k=0; |
Fairy_Paolina | 1:d576753ebd76 | 121 | for(i=0; i<128; i++){ |
Fairy_Paolina | 1:d576753ebd76 | 122 | for(j=0; j<160; j++){ |
Fairy_Paolina | 1:d576753ebd76 | 123 | if(array[i][j] == 1){ // if BLACK |
Fairy_Paolina | 1:d576753ebd76 | 124 | location[k][1]= i; // y |
Fairy_Paolina | 1:d576753ebd76 | 125 | location[k][0]= j; // x |
Fairy_Paolina | 1:d576753ebd76 | 126 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 127 | j=160; |
Fairy_Paolina | 1:d576753ebd76 | 128 | |
Fairy_Paolina | 1:d576753ebd76 | 129 | }}} |
Fairy_Paolina | 1:d576753ebd76 | 130 | prev= location[k-1][0]; // save last j value |
Fairy_Paolina | 1:d576753ebd76 | 131 | |
Fairy_Paolina | 1:d576753ebd76 | 132 | // scan from top to bottom |
Fairy_Paolina | 1:d576753ebd76 | 133 | for(j=prev; j>160; j++){ |
Fairy_Paolina | 1:d576753ebd76 | 134 | for(i=128; i>0; i--){ |
Fairy_Paolina | 1:d576753ebd76 | 135 | if(array[i][j] == 1){ // if BLACK |
Fairy_Paolina | 1:d576753ebd76 | 136 | location[k][1]= i; // y |
Fairy_Paolina | 1:d576753ebd76 | 137 | location[k][0]= j; // x |
Fairy_Paolina | 1:d576753ebd76 | 138 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 139 | i=0; |
Fairy_Paolina | 1:d576753ebd76 | 140 | }}} |
Fairy_Paolina | 1:d576753ebd76 | 141 | |
Fairy_Paolina | 1:d576753ebd76 | 142 | prev= location[k-1][1]; // save last i value |
Fairy_Paolina | 1:d576753ebd76 | 143 | |
Fairy_Paolina | 1:d576753ebd76 | 144 | // scan from right to left |
Fairy_Paolina | 1:d576753ebd76 | 145 | for(i=prev; i>0; i--){ |
Fairy_Paolina | 1:d576753ebd76 | 146 | for(j=159; j>0; j--){ |
Fairy_Paolina | 1:d576753ebd76 | 147 | if(array[i][j] == 1){ // if BLACK |
Fairy_Paolina | 1:d576753ebd76 | 148 | location[k][1]= i; // y |
Fairy_Paolina | 1:d576753ebd76 | 149 | location[k][0]= j; // x |
Fairy_Paolina | 1:d576753ebd76 | 150 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 151 | j=0; |
Fairy_Paolina | 1:d576753ebd76 | 152 | }}} |
Fairy_Paolina | 1:d576753ebd76 | 153 | |
Fairy_Paolina | 1:d576753ebd76 | 154 | prev= location[k-1][0]; // save last j value |
Fairy_Paolina | 1:d576753ebd76 | 155 | |
Fairy_Paolina | 1:d576753ebd76 | 156 | // scan from bottom up |
Fairy_Paolina | 1:d576753ebd76 | 157 | for(j=prev; j>0; j--){ |
Fairy_Paolina | 1:d576753ebd76 | 158 | for(i=0; i<127; i++){ |
Fairy_Paolina | 1:d576753ebd76 | 159 | if(array[i][j] == 1){ // if BLACK |
Fairy_Paolina | 1:d576753ebd76 | 160 | 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])){ |
Fairy_Paolina | 1:d576753ebd76 | 161 | 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])){ |
Fairy_Paolina | 1:d576753ebd76 | 162 | location[k][1]= i; // y |
Fairy_Paolina | 1:d576753ebd76 | 163 | location[k][0]= j; // x |
Fairy_Paolina | 1:d576753ebd76 | 164 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 165 | i=127; |
Fairy_Paolina | 1:d576753ebd76 | 166 | }} |
Fairy_Paolina | 1:d576753ebd76 | 167 | }}} |
Fairy_Paolina | 1:d576753ebd76 | 168 | |
Fairy_Paolina | 1:d576753ebd76 | 169 | |
Fairy_Paolina | 1:d576753ebd76 | 170 | |
Fairy_Paolina | 1:d576753ebd76 | 171 | // save the last k |
Fairy_Paolina | 1:d576753ebd76 | 172 | lastk = k-1; |
Fairy_Paolina | 1:d576753ebd76 | 173 | for(i=0; i< lastk; i++)fprintf(fp, "%d\t%d\n", location[i][0], location[i][1]); |
Fairy_Paolina | 1:d576753ebd76 | 174 | i=0; |
Fairy_Paolina | 1:d576753ebd76 | 175 | k=0; |
Fairy_Paolina | 1:d576753ebd76 | 176 | // find slopes of 8 segments |
Fairy_Paolina | 1:d576753ebd76 | 177 | while(i< lastk){ |
Fairy_Paolina | 1:d576753ebd76 | 178 | |
Fairy_Paolina | 1:d576753ebd76 | 179 | /*if((location[i+1][0] - location[i][0]) == 0) slope[k] =100;// arbitrarily choose a number to differentiante num/0 with 0 |
Fairy_Paolina | 1:d576753ebd76 | 180 | else slope[k]=(location[i+1][1] - location[i][1])/(location[i+1][0] - location[i][0]); |
Fairy_Paolina | 1:d576753ebd76 | 181 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 182 | i++; */ |
Fairy_Paolina | 1:d576753ebd76 | 183 | if((location[i+4][0] - location[i][0]) == 0) slope[k] =100;// arbitrarily choose a number to differentiante num/0 with 0 |
Fairy_Paolina | 1:d576753ebd76 | 184 | else slope[k]=(location[i+4][1] - location[i][1])/(location[i+4][0] - location[i][0]); |
Fairy_Paolina | 1:d576753ebd76 | 185 | k++; |
Fairy_Paolina | 1:d576753ebd76 | 186 | i+=4; |
Fairy_Paolina | 1:d576753ebd76 | 187 | } |
Fairy_Paolina | 1:d576753ebd76 | 188 | lastk=k-1; |
Fairy_Paolina | 1:d576753ebd76 | 189 | |
Fairy_Paolina | 1:d576753ebd76 | 190 | |
Fairy_Paolina | 1:d576753ebd76 | 191 | |
Fairy_Paolina | 1:d576753ebd76 | 192 | i=j=0; |
Fairy_Paolina | 1:d576753ebd76 | 193 | while(i<lastk){// check for slope changes |
Fairy_Paolina | 1:d576753ebd76 | 194 | prev=slope[i]; |
Fairy_Paolina | 1:d576753ebd76 | 195 | printf("%d\n\r",slope[i]); |
Fairy_Paolina | 1:d576753ebd76 | 196 | while(prev == slope[i]){ |
Fairy_Paolina | 1:d576753ebd76 | 197 | i++; |
Fairy_Paolina | 1:d576753ebd76 | 198 | printf("%d \r\n",slope[i]); |
Fairy_Paolina | 1:d576753ebd76 | 199 | if((prev-slope[i])<=3 && (prev-slope[i])>=-3){ |
Fairy_Paolina | 1:d576753ebd76 | 200 | if((prev>0) != (slope[i]<0)){ |
Fairy_Paolina | 1:d576753ebd76 | 201 | if((prev != 0) && (slope[i] != 0))prev=slope[i]; |
Fairy_Paolina | 1:d576753ebd76 | 202 | }}} |
Fairy_Paolina | 1:d576753ebd76 | 203 | |
Fairy_Paolina | 1:d576753ebd76 | 204 | if((prev == slope[i+1]) || (slope[i+1] == slope[i-1]))i++; |
Fairy_Paolina | 1:d576753ebd76 | 205 | else if((slope[i+1] == slope[i+2])&& i<lastk){ |
Fairy_Paolina | 1:d576753ebd76 | 206 | s[sides]=i; |
Fairy_Paolina | 1:d576753ebd76 | 207 | sides++; // if just one number then, skip over |
Fairy_Paolina | 1:d576753ebd76 | 208 | printf("SIDES %d\n\r",sides); |
Fairy_Paolina | 1:d576753ebd76 | 209 | } |
Fairy_Paolina | 1:d576753ebd76 | 210 | i++; |
Fairy_Paolina | 1:d576753ebd76 | 211 | } |
Fairy_Paolina | 1:d576753ebd76 | 212 | // compare last two slopes with beginning two slopes |
Fairy_Paolina | 1:d576753ebd76 | 213 | if((slope[0]==slope[lastk]) || (slope[0]==slope[lastk-1]) || (slope[1]==slope[lastk]) || (slope[1]==slope[lastk-1]))sides--; |
Fairy_Paolina | 1:d576753ebd76 | 214 | fclose(fp); |
Fairy_Paolina | 1:d576753ebd76 | 215 | |
Fairy_Paolina | 1:d576753ebd76 | 216 | // verify shapes |
Fairy_Paolina | 1:d576753ebd76 | 217 | |
Fairy_Paolina | 1:d576753ebd76 | 218 | if(!Verify(slope,s,sides))return 4; |
Fairy_Paolina | 1:d576753ebd76 | 219 | |
Fairy_Paolina | 1:d576753ebd76 | 220 | |
Fairy_Paolina | 1:d576753ebd76 | 221 | if(sides < 4) return 3; // Triange |
Fairy_Paolina | 1:d576753ebd76 | 222 | else if(sides ==4) return 2; // square |
Fairy_Paolina | 1:d576753ebd76 | 223 | else return 1; // circle |
Fairy_Paolina | 1:d576753ebd76 | 224 | |
Fairy_Paolina | 1:d576753ebd76 | 225 | } |
Fairy_Paolina | 1:d576753ebd76 | 226 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Fairy_Paolina | 1:d576753ebd76 | 227 | int CheckShapeInFrame(unsigned char ** Image){ |
Fairy_Paolina | 1:d576753ebd76 | 228 | int i ,j; |
Fairy_Paolina | 1:d576753ebd76 | 229 | |
Fairy_Paolina | 1:d576753ebd76 | 230 | // check that outer edges are white (0) |
Fairy_Paolina | 1:d576753ebd76 | 231 | |
Fairy_Paolina | 1:d576753ebd76 | 232 | //left edge |
Fairy_Paolina | 1:d576753ebd76 | 233 | for(j=0; j<160; j++) if(Image[0][j] != 0) return 2; // move right |
Fairy_Paolina | 1:d576753ebd76 | 234 | //right edge |
Fairy_Paolina | 1:d576753ebd76 | 235 | for(j=0; j<160; j++) if(Image[127][j] != 0) return 1; // move left |
Fairy_Paolina | 1:d576753ebd76 | 236 | //top edge |
Fairy_Paolina | 1:d576753ebd76 | 237 | for(i=0; i<128; i++) if(Image[i][0] != 0) return 4; // move down |
Fairy_Paolina | 1:d576753ebd76 | 238 | //bottom edge |
Fairy_Paolina | 1:d576753ebd76 | 239 | for(i=0; i<128; i++) if(Image[i][159] != 0) return 3; // move up |
Fairy_Paolina | 1:d576753ebd76 | 240 | |
Fairy_Paolina | 1:d576753ebd76 | 241 | return 0; |
Fairy_Paolina | 1:d576753ebd76 | 242 | |
Fairy_Paolina | 1:d576753ebd76 | 243 | } |
Fairy_Paolina | 1:d576753ebd76 | 244 | ///////////////////////////////////////////////////////////////////////////////////////////////////////////// |
Fairy_Paolina | 1:d576753ebd76 | 245 | int GetShape(void){ |
Fairy_Paolina | 1:d576753ebd76 | 246 | char lrfchar=0, end[3]; |
Fairy_Paolina | 1:d576753ebd76 | 247 | int i, j, repeat=0, move=0; |
Fairy_Paolina | 1:d576753ebd76 | 248 | unsigned char c; |
Fairy_Paolina | 1:d576753ebd76 | 249 | unsigned char** Image; |
Fairy_Paolina | 1:d576753ebd76 | 250 | |
Fairy_Paolina | 1:d576753ebd76 | 251 | Image=(unsigned char**)malloc(128*sizeof(unsigned char*)); |
Fairy_Paolina | 1:d576753ebd76 | 252 | for(j=0; j<128; j++){ |
Fairy_Paolina | 1:d576753ebd76 | 253 | Image[j]=(unsigned char*)malloc(160*sizeof(unsigned char)); |
Fairy_Paolina | 1:d576753ebd76 | 254 | } |
Fairy_Paolina | 1:d576753ebd76 | 255 | |
Fairy_Paolina | 1:d576753ebd76 | 256 | //lrf.putc('E'); //Adjust lighting conditions |
Fairy_Paolina | 1:d576753ebd76 | 257 | //wait(.2); |
Fairy_Paolina | 1:d576753ebd76 | 258 | |
Fairy_Paolina | 1:d576753ebd76 | 259 | do{ |
Fairy_Paolina | 1:d576753ebd76 | 260 | |
Fairy_Paolina | 1:d576753ebd76 | 261 | // get grayscale image |
Fairy_Paolina | 1:d576753ebd76 | 262 | lrf.putc('G'); //Take Binary range reading |
Fairy_Paolina | 1:d576753ebd76 | 263 | // read in each byte at a time filling in the Image |
Fairy_Paolina | 1:d576753ebd76 | 264 | // 0= white |
Fairy_Paolina | 1:d576753ebd76 | 265 | // 1= black |
Fairy_Paolina | 1:d576753ebd76 | 266 | for(i=0; i<128; i++){ |
Fairy_Paolina | 1:d576753ebd76 | 267 | for(j=0; j<160; j++){ |
Fairy_Paolina | 1:d576753ebd76 | 268 | c=lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 269 | if(c>80)Image[i][j]= 0; // VERIFY THIS ! WAS 70 |
Fairy_Paolina | 1:d576753ebd76 | 270 | else Image[i][j] =1; |
Fairy_Paolina | 1:d576753ebd76 | 271 | }} |
Fairy_Paolina | 1:d576753ebd76 | 272 | |
Fairy_Paolina | 1:d576753ebd76 | 273 | // for error checking purposes |
Fairy_Paolina | 1:d576753ebd76 | 274 | end[0]=lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 275 | end[1]=lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 276 | end[2]=lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 277 | |
Fairy_Paolina | 1:d576753ebd76 | 278 | move= CheckShapeInFrame(Image); |
Fairy_Paolina | 1:d576753ebd76 | 279 | |
Fairy_Paolina | 1:d576753ebd76 | 280 | if(move == 1) pc.printf("Move Left\r\n"); |
Fairy_Paolina | 1:d576753ebd76 | 281 | else if(move == 2) pc.printf("Move right\r\n"); |
Fairy_Paolina | 1:d576753ebd76 | 282 | else if(move == 3) pc.printf("Move up\r\n"); |
Fairy_Paolina | 1:d576753ebd76 | 283 | else if(move == 4) pc.printf("Move down\r\n"); |
Fairy_Paolina | 1:d576753ebd76 | 284 | else repeat = 1; |
Fairy_Paolina | 1:d576753ebd76 | 285 | |
Fairy_Paolina | 1:d576753ebd76 | 286 | if(!strcmp(end, "END"))pc.printf("Done!\r\n"); |
Fairy_Paolina | 1:d576753ebd76 | 287 | else repeat = 0; |
Fairy_Paolina | 1:d576753ebd76 | 288 | |
Fairy_Paolina | 1:d576753ebd76 | 289 | |
Fairy_Paolina | 1:d576753ebd76 | 290 | //eat CR & ":" command prompt |
Fairy_Paolina | 1:d576753ebd76 | 291 | do { |
Fairy_Paolina | 1:d576753ebd76 | 292 | if(lrf.readable())lrfchar=lrf.getc(); |
Fairy_Paolina | 1:d576753ebd76 | 293 | }while(lrfchar != ':'); |
Fairy_Paolina | 1:d576753ebd76 | 294 | |
Fairy_Paolina | 1:d576753ebd76 | 295 | //i=Shape(Image); |
Fairy_Paolina | 1:d576753ebd76 | 296 | |
Fairy_Paolina | 1:d576753ebd76 | 297 | //if(i==4)repeat =0; |
Fairy_Paolina | 1:d576753ebd76 | 298 | |
Fairy_Paolina | 1:d576753ebd76 | 299 | }while(!repeat); |
Fairy_Paolina | 1:d576753ebd76 | 300 | |
Fairy_Paolina | 1:d576753ebd76 | 301 | |
Fairy_Paolina | 1:d576753ebd76 | 302 | i=Shape(Image); |
Fairy_Paolina | 1:d576753ebd76 | 303 | |
Fairy_Paolina | 1:d576753ebd76 | 304 | for(j=0; j<128; j++){ |
Fairy_Paolina | 1:d576753ebd76 | 305 | free(Image[j]); |
Fairy_Paolina | 1:d576753ebd76 | 306 | } |
Fairy_Paolina | 1:d576753ebd76 | 307 | free(Image); |
Fairy_Paolina | 1:d576753ebd76 | 308 | |
Fairy_Paolina | 1:d576753ebd76 | 309 | return i; |
Fairy_Paolina | 1:d576753ebd76 | 310 | |
Fairy_Paolina | 1:d576753ebd76 | 311 | } |