root / trunk / code / projects / linefollowing / lineFollow.c @ 1983
History | View | Annotate | Download (10.8 KB)
1 | 1864 | azirbel | /**
|
---|---|---|---|
2 | * @file lineFollow.c
|
||
3 | 1931 | djacobs | * @defgroup lineFollwing Line Following
|
4 | 1864 | azirbel | *
|
5 | * Takes care of following a line. Running this program is done by calling the
|
||
6 | * init() function and then the lineFollow(speed) command. However, direct use
|
||
7 | * of this class is discouraged as its behavior is used by lineDrive.c, which
|
||
8 | * extends this class to provide behavior functionality.
|
||
9 | *
|
||
10 | 1931 | djacobs | * @author Dan Jacobs and the Colony Project
|
11 | 1864 | azirbel | * @date 11-1-2010
|
12 | */
|
||
13 | |||
14 | 1841 | djacobs | #include "lineFollow.h" |
15 | |||
16 | 1931 | djacobs | //! The number of bits expected in a barcode
|
17 | 1845 | dgurjar | #define CODESIZE 5 |
18 | 1951 | djacobs | #define LINE_COLOR 200 |
19 | 1841 | djacobs | |
20 | 1983 | azirbel | /**
|
21 | * Helps with debugging.
|
||
22 | * 0 - no debug output.
|
||
23 | * 1 - print out buckets
|
||
24 | * 2 - print out line sensor readings
|
||
25 | */
|
||
26 | #define DBG_LINEFOLLOW 0 |
||
27 | 1951 | djacobs | |
28 | 1842 | djacobs | int countHi = 0; |
29 | int countLo = 0; |
||
30 | int maxAvg, avg;
|
||
31 | 1841 | djacobs | |
32 | 1952 | azirbel | // Everything has a dimension of 2 for left and right readings
|
33 | int barCode[2][ CODESIZE ]; |
||
34 | int barCodePosition[2]={0}; |
||
35 | 1942 | azirbel | |
36 | 1952 | azirbel | int duration[2] = {0}; |
37 | int lastColor[2] = {0}; |
||
38 | 1974 | azirbel | char isReset[2] = {0}; |
39 | 1981 | azirbel | int lastReadings[2][ NUM_READINGS ] = {{0}}; |
40 | 1959 | azirbel | int lastReadingsPtr[2] = {0}; |
41 | 1974 | azirbel | int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} }; |
42 | 1961 | azirbel | int bitColor[2] = {0}; |
43 | 1952 | azirbel | |
44 | 1851 | djacobs | int turnDistance=0; |
45 | 1931 | djacobs | //! Counts the number of full line readings before we determine an intersection
|
46 | 1853 | djacobs | int intersectionFilter=0; |
47 | 1851 | djacobs | int disableBarCode=0; |
48 | |||
49 | 1931 | djacobs | //! Keeps track of where the encoder of one motor started, for use in turns.
|
50 | int encoderStart = -1; |
||
51 | int encoderReset = 0; // 0 if encoderStart has no value set |
||
52 | 1851 | djacobs | |
53 | 1931 | djacobs | |
54 | 1841 | djacobs | void lineFollow_init()
|
55 | { |
||
56 | 1981 | azirbel | int i, j;
|
57 | |||
58 | 1841 | djacobs | analog_init(0);
|
59 | 1959 | azirbel | encoders_init(); |
60 | 1841 | djacobs | lost = 0;
|
61 | 1853 | djacobs | intersectionFilter=0;
|
62 | 1851 | djacobs | disableBarCode=0;
|
63 | 1981 | azirbel | |
64 | for(i=0; i<2; i++) |
||
65 | { |
||
66 | for(j=0; j<NUM_READINGS; j++) |
||
67 | { |
||
68 | lastReadings[i][j] = BAD_READING; |
||
69 | } |
||
70 | } |
||
71 | |||
72 | //numLast = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} };
|
||
73 | 1841 | djacobs | } |
74 | |||
75 | |||
76 | 1864 | azirbel | /**
|
77 | * Follows a line at the given speed.
|
||
78 | * @param speed The speed with which to follow the line.
|
||
79 | */
|
||
80 | 1851 | djacobs | int lineFollow(int speed) |
81 | 1841 | djacobs | { |
82 | 1851 | djacobs | int colors[5]; |
83 | 1841 | djacobs | int position;
|
84 | |||
85 | |||
86 | updateLine(colors); |
||
87 | position = lineLocate(colors); |
||
88 | |||
89 | //not on line
|
||
90 | if(position == NOLINE)
|
||
91 | { |
||
92 | 1931 | djacobs | if(lost++ > 20) |
93 | 1841 | djacobs | { |
94 | orb2_set_color(GREEN); |
||
95 | motors_off(); |
||
96 | return LINELOST;
|
||
97 | } |
||
98 | } |
||
99 | 1851 | djacobs | else if(position == FULL_LINE) |
100 | { |
||
101 | 1931 | djacobs | if(intersectionFilter++ > 4) |
102 | 1851 | djacobs | { |
103 | orb2_set_color(RED); |
||
104 | 1952 | azirbel | barCodePosition[0]=0; |
105 | 1959 | azirbel | barCodePosition[1]=0; |
106 | 1853 | djacobs | disableBarCode=50;
|
107 | 1851 | djacobs | } |
108 | } |
||
109 | 1841 | djacobs | //on line
|
110 | else
|
||
111 | { |
||
112 | position*=30;
|
||
113 | orb2_set_color(ORB_OFF); |
||
114 | 1851 | djacobs | motorLeft(min(speed+position, 255));
|
115 | motorRight(min(speed-position, 255));
|
||
116 | 1841 | djacobs | lost=0;
|
117 | 1853 | djacobs | intersectionFilter=0;
|
118 | 1841 | djacobs | } |
119 | 1851 | djacobs | |
120 | 1942 | azirbel | // If we're running over a line, stop reading barcodes for a sec
|
121 | if(disableBarCode-- > 0) |
||
122 | 1851 | djacobs | { |
123 | 1942 | azirbel | // Return intersection once we cross the line
|
124 | if(disableBarCode) return NOBARCODE; |
||
125 | 1853 | djacobs | return INTERSECTION;
|
126 | 1851 | djacobs | } |
127 | 1842 | djacobs | updateBarCode(); |
128 | return getBarCode();
|
||
129 | 1841 | djacobs | } |
130 | |||
131 | 1851 | djacobs | |
132 | 1864 | azirbel | /**
|
133 | * Implements the left merge, assuming a line exists to the left. Works by
|
||
134 | * turning off the line at an increasing angle and waiting to hit another line
|
||
135 | * on the left.
|
||
136 | */
|
||
137 | 1854 | djacobs | int mergeLeft()
|
138 | 1851 | djacobs | { |
139 | 1854 | djacobs | motor_l_set(FORWARD, 200);
|
140 | if(turnDistance!=21)motor_r_set(FORWARD, 230); |
||
141 | else motor_r_set(FORWARD, 210); |
||
142 | 1851 | djacobs | int colors[5]; |
143 | updateLine(colors); |
||
144 | int position = lineLocate(colors);
|
||
145 | 1854 | djacobs | if(position>3 || position<-3)turnDistance++; |
146 | 1853 | djacobs | |
147 | 1854 | djacobs | if(turnDistance>20) |
148 | 1853 | djacobs | { |
149 | 1854 | djacobs | turnDistance=21;
|
150 | |||
151 | 1853 | djacobs | if(position<3 && position>-3) |
152 | { |
||
153 | turnDistance = 0;
|
||
154 | return 0; |
||
155 | } |
||
156 | } |
||
157 | 1851 | djacobs | return 1; |
158 | } |
||
159 | |||
160 | 1854 | djacobs | |
161 | 1864 | azirbel | /**
|
162 | * Implements the right merge, assuming a line exists to the right. Works by
|
||
163 | * turning off the line at an increasing angle and waiting to hit another line
|
||
164 | * on the right.
|
||
165 | */
|
||
166 | 1854 | djacobs | int mergeRight()
|
167 | { |
||
168 | motor_r_set(FORWARD, 200);
|
||
169 | if(turnDistance!=21)motor_l_set(FORWARD, 230); |
||
170 | else motor_l_set(FORWARD, 210); |
||
171 | int colors[5]; |
||
172 | updateLine(colors); |
||
173 | int position = lineLocate(colors);
|
||
174 | if(position>3 || position<-3)turnDistance++; |
||
175 | |||
176 | if(turnDistance>20) |
||
177 | { |
||
178 | turnDistance=21;
|
||
179 | |||
180 | if(position<3 && position>-3) |
||
181 | { |
||
182 | turnDistance = 0;
|
||
183 | return 0; |
||
184 | } |
||
185 | } |
||
186 | return 1; |
||
187 | } |
||
188 | |||
189 | |||
190 | |||
191 | 1864 | azirbel | /**
|
192 | * Turns left at a cross of two lines. Assumes that we are at lines in a cross
|
||
193 | * pattern, and turns until it sets straight on the new line.
|
||
194 | 1931 | djacobs | * @return 0 if turn finishes otherwise return 1
|
195 | 1864 | azirbel | */
|
196 | 1854 | djacobs | int turnLeft()
|
197 | { |
||
198 | 1931 | djacobs | /*motor_l_set(BACKWARD, 200);
|
199 | 1854 | djacobs | motor_r_set(FORWARD, 200);
|
200 | int colors[5];
|
||
201 | updateLine(colors);
|
||
202 | int position = lineLocate(colors);
|
||
203 | if(position>2 || position<-2)turnDistance++;
|
||
204 | if(turnDistance>1)
|
||
205 | {
|
||
206 | if(position<3 && position>-3)
|
||
207 | {
|
||
208 | turnDistance = 0;
|
||
209 | return 0;
|
||
210 | }
|
||
211 | }
|
||
212 | 1931 | djacobs | return 1;*/
|
213 | |||
214 | motor_l_set(BACKWARD,200);
|
||
215 | motor_r_set(FORWARD,200);
|
||
216 | if(!encoderReset)
|
||
217 | { |
||
218 | encoderStart = encoder_get_x(RIGHT); |
||
219 | encoderReset = 1;
|
||
220 | } |
||
221 | |||
222 | if(encoder_get_x(RIGHT) < encoderStart)
|
||
223 | { |
||
224 | encoderStart = 0;
|
||
225 | // Temporary: display an "error message" in case of overflow.
|
||
226 | // Using this for debugging, take it out soon!
|
||
227 | motor_l_set(FORWARD,0);
|
||
228 | motor_r_set(FORWARD,0);
|
||
229 | 1959 | azirbel | //orb_set_color(WHITE);
|
230 | 1931 | djacobs | delay_ms(2000);
|
231 | } |
||
232 | |||
233 | if(encoder_get_x(RIGHT) - encoderStart > 300) |
||
234 | { |
||
235 | encoderReset = 0;
|
||
236 | return 0; |
||
237 | } |
||
238 | 1854 | djacobs | return 1; |
239 | } |
||
240 | |||
241 | |||
242 | |||
243 | 1864 | azirbel | /**
|
244 | * Turns right at a cross of two lines. Assumes that we are at lines in a cross
|
||
245 | * pattern, and turns until it sets straight on the new line.
|
||
246 | 1931 | djacobs | * @return 0 if the turn finishes otherwise return 1
|
247 | 1864 | azirbel | */
|
248 | 1851 | djacobs | int turnRight()
|
249 | { |
||
250 | motor_r_set(BACKWARD, 200);
|
||
251 | motor_l_set(FORWARD, 200);
|
||
252 | int colors[5]; |
||
253 | updateLine(colors); |
||
254 | int position = lineLocate(colors);
|
||
255 | 1853 | djacobs | if(position>2 || position<-2)turnDistance++; |
256 | if(turnDistance>1) |
||
257 | { |
||
258 | if(position<3 && position>-3) |
||
259 | { |
||
260 | turnDistance = 0;
|
||
261 | return 0; |
||
262 | } |
||
263 | } |
||
264 | return 1; |
||
265 | 1851 | djacobs | } |
266 | |||
267 | |||
268 | |||
269 | int getBarCode()
|
||
270 | { |
||
271 | 1959 | azirbel | if(barCodePosition[1] != CODESIZE) |
272 | 1942 | azirbel | return NOBARCODE ;
|
273 | else
|
||
274 | { |
||
275 | int temp = 0; |
||
276 | for(int i=0; i<CODESIZE; i++) |
||
277 | 1959 | azirbel | temp += (barCode[1][i] << i);
|
278 | barCodePosition[1] = 0; |
||
279 | 1942 | azirbel | return temp;
|
280 | } |
||
281 | 1842 | djacobs | } |
282 | 1841 | djacobs | |
283 | |||
284 | |||
285 | void updateLine(int* values) |
||
286 | { |
||
287 | for(int i = 0; i<5; i++) |
||
288 | 1951 | djacobs | values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
|
289 | 1841 | djacobs | } |
290 | |||
291 | |||
292 | |||
293 | int lineLocate(int* colors) |
||
294 | { |
||
295 | int i;
|
||
296 | int wsum = 0; |
||
297 | int count=0; |
||
298 | |||
299 | for(i = 0; i<5; i++) |
||
300 | { |
||
301 | 1845 | dgurjar | count += colors[i]/2;
|
302 | 1841 | djacobs | wsum += (i)*colors[i]; |
303 | } |
||
304 | if(count==0) |
||
305 | 1851 | djacobs | return NOLINE;
|
306 | if(count==5) |
||
307 | return FULL_LINE;
|
||
308 | 1931 | djacobs | return (wsum/count)-4; // Subtract 4 to center the index around the center. |
309 | 1841 | djacobs | } |
310 | |||
311 | 1959 | azirbel | void addToBuckets(int curColor, int i) |
312 | { |
||
313 | int oldest = lastReadings[i][lastReadingsPtr[i]];
|
||
314 | numLast[i][oldest]--; |
||
315 | lastReadings[i][lastReadingsPtr[i]] = curColor; |
||
316 | lastReadingsPtr[i] = (lastReadingsPtr[i]+1) % NUM_READINGS;
|
||
317 | numLast[i][curColor]++; |
||
318 | 1841 | djacobs | |
319 | 1983 | azirbel | if(DBG_LINEFOLLOW == 1) |
320 | 1959 | azirbel | { |
321 | 1983 | azirbel | usb_puts("LP[");
|
322 | usb_puti(i); |
||
323 | 1959 | azirbel | usb_puts("]: ");
|
324 | 1983 | azirbel | usb_puti(lastReadingsPtr[i]); |
325 | usb_puts(", oldest: ");
|
||
326 | usb_puti(oldest); |
||
327 | usb_puts(", Totals: ");
|
||
328 | |||
329 | for(int j=0; j<=3; j++) |
||
330 | { |
||
331 | usb_puts("[");
|
||
332 | usb_puti(j); |
||
333 | usb_puts("]: ");
|
||
334 | usb_puti(numLast[i][j]); |
||
335 | usb_puts(" ");
|
||
336 | } |
||
337 | usb_puts("\t ");
|
||
338 | usb_puts("\n");
|
||
339 | 1959 | azirbel | } |
340 | } |
||
341 | |||
342 | 1945 | azirbel | void updateBarCode()
|
343 | 1942 | azirbel | { |
344 | // USING THESE GLOBAL VARIABLES
|
||
345 | // global int duration = 0;
|
||
346 | // global int lastColor = 0;
|
||
347 | // global int barCodePosition = 0;
|
||
348 | // global char isReset = 0;
|
||
349 | |||
350 | 1959 | azirbel | // Just uses one sensor for now
|
351 | for(int i = /*RIGHT*/LEFT_SENSOR; i <= LEFT_SENSOR; i++) |
||
352 | 1945 | azirbel | { |
353 | 1952 | azirbel | // Add 6 to convert left (1) and right (0) to sensor 6 and 7
|
354 | int curReading = read_line(i + 6); |
||
355 | int curColor;
|
||
356 | 1945 | azirbel | |
357 | 1952 | azirbel | if(curReading > BLACK_THRESHOLD)
|
358 | 1959 | azirbel | { |
359 | 1952 | azirbel | curColor = LBLACK; |
360 | 1959 | azirbel | } |
361 | 1952 | azirbel | else if(curReading < GREY_THRESHOLD) |
362 | 1959 | azirbel | { |
363 | 1952 | azirbel | curColor = LWHITE; |
364 | 1959 | azirbel | } |
365 | 1952 | azirbel | else
|
366 | 1959 | azirbel | { |
367 | 1952 | azirbel | curColor = LGREY; |
368 | 1959 | azirbel | } |
369 | 1945 | azirbel | |
370 | 1983 | azirbel | // Keep track of this reading
|
371 | 1959 | azirbel | addToBuckets(curColor, i); |
372 | |||
373 | 1952 | azirbel | // Just an error check
|
374 | if(barCodePosition[i] > CODESIZE)
|
||
375 | { |
||
376 | barCodePosition[i] = 0;
|
||
377 | } |
||
378 | 1945 | azirbel | |
379 | 1961 | azirbel | // We now edit curColor to use the majority of the last buckets.
|
380 | 1974 | azirbel | if(numLast[i][1] > NUM_READINGS / 2) |
381 | 1952 | azirbel | { |
382 | 1974 | azirbel | curColor = LGREY; |
383 | 1952 | azirbel | } |
384 | 1974 | azirbel | else if(numLast[i][2] > NUM_READINGS / 2) |
385 | { |
||
386 | curColor = LBLACK; |
||
387 | } |
||
388 | else if(numLast[i][0] > NUM_READINGS / 2) |
||
389 | { |
||
390 | curColor = LWHITE; |
||
391 | duration[i]++; |
||
392 | } |
||
393 | else
|
||
394 | { |
||
395 | 1981 | azirbel | curColor = BAD_READING; |
396 | 1974 | azirbel | } |
397 | 1942 | azirbel | |
398 | 1983 | azirbel | // Print out the current reading and label, if in debug mode
|
399 | if(DBG_LINEFOLLOW == 2) |
||
400 | 1974 | azirbel | { |
401 | 1983 | azirbel | switch(curColor)
|
402 | { |
||
403 | case LBLACK: usb_puts("LBLACK.\t"); break; |
||
404 | case LGREY: usb_puts("LGREY.\t\t"); break; |
||
405 | case LWHITE: usb_puts("LWHITE.\t\t\t"); break; |
||
406 | } |
||
407 | usb_puti(curReading); |
||
408 | usb_puts("\n");
|
||
409 | 1974 | azirbel | } |
410 | |||
411 | 1981 | azirbel | if(curColor != BAD_READING)
|
412 | 1952 | azirbel | { |
413 | // Now we assume our reading is significant - a bit, or a white space
|
||
414 | 1942 | azirbel | |
415 | 1961 | azirbel | if(bitColor[i] == LGREY || bitColor[i] == LWHITE)
|
416 | bitColor[i] = curColor; |
||
417 | |||
418 | 1952 | azirbel | // Only read a value if we have read 0 first (isReset == 1)
|
419 | if(isReset[i] && (curColor == LBLACK || curColor == LGREY) )
|
||
420 | { |
||
421 | isReset[i] = 0;
|
||
422 | 1959 | azirbel | duration[i] = 0;
|
423 | 1952 | azirbel | } |
424 | else if(curColor == LWHITE) |
||
425 | { |
||
426 | 1983 | azirbel | if(!isReset[i])
|
427 | 1959 | azirbel | { |
428 | 1961 | azirbel | barCode[i][barCodePosition[i]++] = |
429 | (bitColor[i] == LBLACK) ? 1 : 0; |
||
430 | usb_puts("Reset. Read bit: ");
|
||
431 | 1974 | azirbel | usb_puts(((bitColor[i] == LBLACK) ? "BLACK" : "GREY")); |
432 | 1961 | azirbel | usb_puts("\n");
|
433 | |||
434 | bitColor[i] = LWHITE; |
||
435 | 1959 | azirbel | } |
436 | 1952 | azirbel | isReset[i] = 1;
|
437 | 1959 | azirbel | orb_set(0, 0, 0); |
438 | 1952 | azirbel | } |
439 | 1942 | azirbel | } |
440 | 1959 | azirbel | |
441 | if(curColor == LWHITE && duration[i] > TIMEOUT_DURATION
|
||
442 | && barCodePosition[i] != 0)
|
||
443 | 1942 | azirbel | { |
444 | 1952 | azirbel | usb_puts("TIMED OUT. BARCODE READER RESET.\n");
|
445 | 1959 | azirbel | usb_puts("Encoders: ");
|
446 | usb_puti(encoder_get_dx(LEFT)); |
||
447 | usb_puts(", ");
|
||
448 | usb_puti(encoder_get_dx(RIGHT)); |
||
449 | usb_puts("\n");
|
||
450 | 1952 | azirbel | barCodePosition[i] = 0;
|
451 | duration[i] = 0;
|
||
452 | isReset[i] = 1;
|
||
453 | 1942 | azirbel | } |
454 | } |
||
455 | 1945 | azirbel | } |
456 | 1942 | azirbel | |
457 | 1854 | djacobs | |
458 | 1841 | djacobs | int min(int x, int y){return x>y ? y : x;} |
459 | 1842 | djacobs | int max(int x, int y){return x<y ? y : x;} |
460 | 1841 | djacobs | |
461 | void motorLeft(int speed){ |
||
462 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
463 | 1841 | djacobs | } |
464 | |||
465 | void motorRight(int speed){ |
||
466 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
467 | 1841 | djacobs | } |