root / trunk / code / projects / linefollowing / lineFollow.c @ 1951
History | View | Annotate | Download (8.67 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 | 1951 | djacobs | |
21 | 1842 | djacobs | int countHi = 0; |
22 | int countLo = 0; |
||
23 | int maxAvg, avg;
|
||
24 | int barCode[ CODESIZE ];
|
||
25 | int barCodePosition=0; |
||
26 | 1841 | djacobs | |
27 | 1942 | azirbel | int duration = 0; |
28 | int lastColor = 0; |
||
29 | char isReset = 0; |
||
30 | |||
31 | 1851 | djacobs | int turnDistance=0; |
32 | 1931 | djacobs | //! Counts the number of full line readings before we determine an intersection
|
33 | 1853 | djacobs | int intersectionFilter=0; |
34 | 1851 | djacobs | int disableBarCode=0; |
35 | |||
36 | 1931 | djacobs | //! Keeps track of where the encoder of one motor started, for use in turns.
|
37 | int encoderStart = -1; |
||
38 | int encoderReset = 0; // 0 if encoderStart has no value set |
||
39 | 1851 | djacobs | |
40 | 1931 | djacobs | |
41 | 1841 | djacobs | void lineFollow_init()
|
42 | { |
||
43 | analog_init(0);
|
||
44 | lost = 0;
|
||
45 | 1853 | djacobs | intersectionFilter=0;
|
46 | 1851 | djacobs | disableBarCode=0;
|
47 | 1841 | djacobs | } |
48 | |||
49 | |||
50 | 1864 | azirbel | /**
|
51 | * Follows a line at the given speed.
|
||
52 | * @param speed The speed with which to follow the line.
|
||
53 | */
|
||
54 | 1851 | djacobs | int lineFollow(int speed) |
55 | 1841 | djacobs | { |
56 | 1851 | djacobs | int colors[5]; |
57 | 1841 | djacobs | int position;
|
58 | |||
59 | |||
60 | updateLine(colors); |
||
61 | position = lineLocate(colors); |
||
62 | |||
63 | //not on line
|
||
64 | if(position == NOLINE)
|
||
65 | { |
||
66 | 1931 | djacobs | if(lost++ > 20) |
67 | 1841 | djacobs | { |
68 | orb2_set_color(GREEN); |
||
69 | motors_off(); |
||
70 | return LINELOST;
|
||
71 | } |
||
72 | } |
||
73 | 1851 | djacobs | else if(position == FULL_LINE) |
74 | { |
||
75 | 1931 | djacobs | if(intersectionFilter++ > 4) |
76 | 1851 | djacobs | { |
77 | orb2_set_color(RED); |
||
78 | barCodePosition=0;
|
||
79 | 1853 | djacobs | disableBarCode=50;
|
80 | 1851 | djacobs | } |
81 | } |
||
82 | 1841 | djacobs | //on line
|
83 | else
|
||
84 | { |
||
85 | position*=30;
|
||
86 | orb2_set_color(ORB_OFF); |
||
87 | 1851 | djacobs | motorLeft(min(speed+position, 255));
|
88 | motorRight(min(speed-position, 255));
|
||
89 | 1841 | djacobs | lost=0;
|
90 | 1853 | djacobs | intersectionFilter=0;
|
91 | 1841 | djacobs | } |
92 | 1851 | djacobs | |
93 | 1942 | azirbel | // If we're running over a line, stop reading barcodes for a sec
|
94 | if(disableBarCode-- > 0) |
||
95 | 1851 | djacobs | { |
96 | 1942 | azirbel | // Return intersection once we cross the line
|
97 | if(disableBarCode) return NOBARCODE; |
||
98 | 1853 | djacobs | return INTERSECTION;
|
99 | 1851 | djacobs | } |
100 | 1842 | djacobs | updateBarCode(); |
101 | return getBarCode();
|
||
102 | 1841 | djacobs | } |
103 | |||
104 | 1851 | djacobs | |
105 | 1864 | azirbel | /**
|
106 | * Implements the left merge, assuming a line exists to the left. Works by
|
||
107 | * turning off the line at an increasing angle and waiting to hit another line
|
||
108 | * on the left.
|
||
109 | */
|
||
110 | 1854 | djacobs | int mergeLeft()
|
111 | 1851 | djacobs | { |
112 | 1854 | djacobs | motor_l_set(FORWARD, 200);
|
113 | if(turnDistance!=21)motor_r_set(FORWARD, 230); |
||
114 | else motor_r_set(FORWARD, 210); |
||
115 | 1851 | djacobs | int colors[5]; |
116 | updateLine(colors); |
||
117 | int position = lineLocate(colors);
|
||
118 | 1854 | djacobs | if(position>3 || position<-3)turnDistance++; |
119 | 1853 | djacobs | |
120 | 1854 | djacobs | if(turnDistance>20) |
121 | 1853 | djacobs | { |
122 | 1854 | djacobs | turnDistance=21;
|
123 | |||
124 | 1853 | djacobs | if(position<3 && position>-3) |
125 | { |
||
126 | turnDistance = 0;
|
||
127 | return 0; |
||
128 | } |
||
129 | } |
||
130 | 1851 | djacobs | return 1; |
131 | } |
||
132 | |||
133 | 1854 | djacobs | |
134 | 1864 | azirbel | /**
|
135 | * Implements the right merge, assuming a line exists to the right. Works by
|
||
136 | * turning off the line at an increasing angle and waiting to hit another line
|
||
137 | * on the right.
|
||
138 | */
|
||
139 | 1854 | djacobs | int mergeRight()
|
140 | { |
||
141 | motor_r_set(FORWARD, 200);
|
||
142 | if(turnDistance!=21)motor_l_set(FORWARD, 230); |
||
143 | else motor_l_set(FORWARD, 210); |
||
144 | int colors[5]; |
||
145 | updateLine(colors); |
||
146 | int position = lineLocate(colors);
|
||
147 | if(position>3 || position<-3)turnDistance++; |
||
148 | |||
149 | if(turnDistance>20) |
||
150 | { |
||
151 | turnDistance=21;
|
||
152 | |||
153 | if(position<3 && position>-3) |
||
154 | { |
||
155 | turnDistance = 0;
|
||
156 | return 0; |
||
157 | } |
||
158 | } |
||
159 | return 1; |
||
160 | } |
||
161 | |||
162 | |||
163 | |||
164 | 1864 | azirbel | /**
|
165 | * Turns left at a cross of two lines. Assumes that we are at lines in a cross
|
||
166 | * pattern, and turns until it sets straight on the new line.
|
||
167 | 1931 | djacobs | * @return 0 if turn finishes otherwise return 1
|
168 | 1864 | azirbel | */
|
169 | 1854 | djacobs | int turnLeft()
|
170 | { |
||
171 | 1931 | djacobs | /*motor_l_set(BACKWARD, 200);
|
172 | 1854 | djacobs | motor_r_set(FORWARD, 200);
|
173 | int colors[5];
|
||
174 | updateLine(colors);
|
||
175 | int position = lineLocate(colors);
|
||
176 | if(position>2 || position<-2)turnDistance++;
|
||
177 | if(turnDistance>1)
|
||
178 | {
|
||
179 | if(position<3 && position>-3)
|
||
180 | {
|
||
181 | turnDistance = 0;
|
||
182 | return 0;
|
||
183 | }
|
||
184 | }
|
||
185 | 1931 | djacobs | return 1;*/
|
186 | |||
187 | motor_l_set(BACKWARD,200);
|
||
188 | motor_r_set(FORWARD,200);
|
||
189 | if(!encoderReset)
|
||
190 | { |
||
191 | encoderStart = encoder_get_x(RIGHT); |
||
192 | encoderReset = 1;
|
||
193 | } |
||
194 | |||
195 | if(encoder_get_x(RIGHT) < encoderStart)
|
||
196 | { |
||
197 | encoderStart = 0;
|
||
198 | // Temporary: display an "error message" in case of overflow.
|
||
199 | // Using this for debugging, take it out soon!
|
||
200 | motor_l_set(FORWARD,0);
|
||
201 | motor_r_set(FORWARD,0);
|
||
202 | orb_set_color(WHITE); |
||
203 | delay_ms(2000);
|
||
204 | } |
||
205 | |||
206 | if(encoder_get_x(RIGHT) - encoderStart > 300) |
||
207 | { |
||
208 | encoderReset = 0;
|
||
209 | return 0; |
||
210 | } |
||
211 | 1854 | djacobs | return 1; |
212 | } |
||
213 | |||
214 | |||
215 | |||
216 | 1864 | azirbel | /**
|
217 | * Turns right at a cross of two lines. Assumes that we are at lines in a cross
|
||
218 | * pattern, and turns until it sets straight on the new line.
|
||
219 | 1931 | djacobs | * @return 0 if the turn finishes otherwise return 1
|
220 | 1864 | azirbel | */
|
221 | 1851 | djacobs | int turnRight()
|
222 | { |
||
223 | motor_r_set(BACKWARD, 200);
|
||
224 | motor_l_set(FORWARD, 200);
|
||
225 | int colors[5]; |
||
226 | updateLine(colors); |
||
227 | int position = lineLocate(colors);
|
||
228 | 1853 | djacobs | if(position>2 || position<-2)turnDistance++; |
229 | if(turnDistance>1) |
||
230 | { |
||
231 | if(position<3 && position>-3) |
||
232 | { |
||
233 | turnDistance = 0;
|
||
234 | return 0; |
||
235 | } |
||
236 | } |
||
237 | return 1; |
||
238 | 1851 | djacobs | } |
239 | |||
240 | |||
241 | |||
242 | int getBarCode()
|
||
243 | { |
||
244 | 1942 | azirbel | if(barCodePosition != CODESIZE)
|
245 | return NOBARCODE ;
|
||
246 | else
|
||
247 | { |
||
248 | int temp = 0; |
||
249 | for(int i=0; i<CODESIZE; i++) |
||
250 | temp += (barCode[i] << i); |
||
251 | barCodePosition = 0;
|
||
252 | return temp;
|
||
253 | } |
||
254 | 1842 | djacobs | } |
255 | 1841 | djacobs | |
256 | |||
257 | |||
258 | void updateLine(int* values) |
||
259 | { |
||
260 | for(int i = 0; i<5; i++) |
||
261 | 1951 | djacobs | values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
|
262 | 1841 | djacobs | } |
263 | |||
264 | |||
265 | |||
266 | int lineLocate(int* colors) |
||
267 | { |
||
268 | int i;
|
||
269 | int wsum = 0; |
||
270 | int count=0; |
||
271 | |||
272 | for(i = 0; i<5; i++) |
||
273 | { |
||
274 | 1845 | dgurjar | count += colors[i]/2;
|
275 | 1841 | djacobs | wsum += (i)*colors[i]; |
276 | } |
||
277 | if(count==0) |
||
278 | 1851 | djacobs | return NOLINE;
|
279 | if(count==5) |
||
280 | return FULL_LINE;
|
||
281 | 1931 | djacobs | return (wsum/count)-4; // Subtract 4 to center the index around the center. |
282 | 1841 | djacobs | } |
283 | |||
284 | |||
285 | 1942 | azirbel | // new version by Alex
|
286 | 1945 | azirbel | void updateBarCode()
|
287 | 1942 | azirbel | { |
288 | // USING THESE GLOBAL VARIABLES
|
||
289 | // global int duration = 0;
|
||
290 | // global int lastColor = 0;
|
||
291 | // global int barCodePosition = 0;
|
||
292 | // global char isReset = 0;
|
||
293 | |||
294 | 1945 | azirbel | // Average the readings of the last 2 sensors
|
295 | if(read_line(6) - read_line(7) > 150 || read_line(6) - read_line(7) < -150) |
||
296 | { |
||
297 | return;
|
||
298 | } |
||
299 | |||
300 | int curReading = (read_line(6) + read_line(7)) / 2; |
||
301 | int curColor;
|
||
302 | |||
303 | if(curReading > BLACK_THRESHOLD)
|
||
304 | curColor = LBLACK; |
||
305 | else if(curReading < GREY_THRESHOLD) |
||
306 | curColor = LWHITE; |
||
307 | else
|
||
308 | curColor = LGREY; |
||
309 | |||
310 | 1942 | azirbel | // Just an error check
|
311 | if(barCodePosition > CODESIZE)
|
||
312 | { |
||
313 | barCodePosition = 0;
|
||
314 | } |
||
315 | |||
316 | 1945 | azirbel | // We are only interested in consecutive color values
|
317 | 1942 | azirbel | if(curColor == lastColor)
|
318 | { |
||
319 | duration++; |
||
320 | } |
||
321 | else
|
||
322 | { |
||
323 | duration = 0;
|
||
324 | lastColor = curColor; |
||
325 | } |
||
326 | |||
327 | if(duration > MAX_DURATION)
|
||
328 | { |
||
329 | // Now we assume our reading is significant - a bit, or a white space
|
||
330 | |||
331 | // Only read a value if we have read 0 first (isReset == 1)
|
||
332 | if(isReset && (curColor == LBLACK || curColor == LGREY) )
|
||
333 | { |
||
334 | isReset = 0;
|
||
335 | 1945 | azirbel | usb_puts("Read barcode bit: ");
|
336 | usb_puti(barCodePosition); |
||
337 | usb_puts(" = ");
|
||
338 | usb_puti(curColor); |
||
339 | usb_puts(", curReading = ");
|
||
340 | usb_puti(curReading); |
||
341 | usb_puts(".\n");
|
||
342 | 1942 | azirbel | |
343 | barCode[barCodePosition++] = (curColor == LBLACK) ? 1 : 0; |
||
344 | } |
||
345 | else if(curColor == LWHITE) |
||
346 | { |
||
347 | isReset = 1;
|
||
348 | } |
||
349 | } |
||
350 | 1945 | azirbel | if(duration > TIMEOUT_DURATION && barCodePosition != 0) |
351 | { |
||
352 | usb_puts("TIMED OUT. BARCODE READER RESET.\n");
|
||
353 | barCodePosition = 0;
|
||
354 | duration = 0;
|
||
355 | } |
||
356 | } |
||
357 | 1942 | azirbel | |
358 | 1945 | azirbel | // Dan's version
|
359 | /*void updateBarCode()
|
||
360 | 1841 | djacobs | {
|
361 | 1931 | djacobs | //! Note: currently only uses one of the barcode sensors.
|
362 | 1854 | djacobs | |
363 | 1841 | djacobs | //maps the sensors to the analog input ports
|
364 | int ports[2] = {8,1};
|
||
365 | 1842 | djacobs | int current[2];
|
366 | 1854 | djacobs | // current[0] = analog_get10(ports[0]);
|
367 | 1942 | azirbel | //current[1] = analog_get10(ports[1]);
|
368 | current[1] = read_line(6);
|
||
369 | 1842 | djacobs | |
370 | 1942 | azirbel | if(current[1] > 150)
|
371 | 1842 | djacobs | {
|
372 | 1942 | azirbel | if(countHi++ == 0)
|
373 | 1842 | djacobs | {
|
374 | avg = 500;
|
||
375 | maxAvg = 500;
|
||
376 | }
|
||
377 | else
|
||
378 | {
|
||
379 | avg = 3*avg + current[1];
|
||
380 | avg/=4;
|
||
381 | maxAvg = max(maxAvg, avg);
|
||
382 | }
|
||
383 | }
|
||
384 | 1942 | azirbel | else if(countHi > 5)
|
385 | 1842 | djacobs | {
|
386 | 1942 | azirbel | if(countLo++ > 15)
|
387 | 1842 | djacobs | {
|
388 | 1942 | azirbel | countHi = countLo = 0;
|
389 | if(maxAvg > 825)
|
||
390 | orb1_set_color(RED);
|
||
391 | 1845 | dgurjar | else orb1_set_color(BLUE);
|
392 | 1851 | djacobs | barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
|
393 | 1842 | djacobs | }
|
394 | }
|
||
395 | else countHi/=2;
|
||
396 | if(countHi==0)countLo=0;
|
||
397 | 1945 | azirbel | }*/
|
398 | 1841 | djacobs | |
399 | |||
400 | int min(int x, int y){return x>y ? y : x;} |
||
401 | 1842 | djacobs | int max(int x, int y){return x<y ? y : x;} |
402 | 1841 | djacobs | |
403 | void motorLeft(int speed){ |
||
404 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
405 | 1841 | djacobs | } |
406 | |||
407 | void motorRight(int speed){ |
||
408 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
409 | 1841 | djacobs | } |