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