root / trunk / code / projects / linefollowing / lineFollow.c @ 1931
History | View | Annotate | Download (6.58 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 | 1841 | djacobs | |
19 | 1842 | djacobs | int countHi = 0; |
20 | int countLo = 0; |
||
21 | int maxAvg, avg;
|
||
22 | int barCode[ CODESIZE ];
|
||
23 | int barCodePosition=0; |
||
24 | 1841 | djacobs | |
25 | 1851 | djacobs | int turnDistance=0; |
26 | 1931 | djacobs | //! Counts the number of full line readings before we determine an intersection
|
27 | 1853 | djacobs | int intersectionFilter=0; |
28 | 1851 | djacobs | int disableBarCode=0; |
29 | |||
30 | 1931 | djacobs | //! Keeps track of where the encoder of one motor started, for use in turns.
|
31 | int encoderStart = -1; |
||
32 | int encoderReset = 0; // 0 if encoderStart has no value set |
||
33 | 1851 | djacobs | |
34 | 1931 | djacobs | |
35 | 1841 | djacobs | void lineFollow_init()
|
36 | { |
||
37 | analog_init(0);
|
||
38 | lost = 0;
|
||
39 | 1853 | djacobs | intersectionFilter=0;
|
40 | 1851 | djacobs | disableBarCode=0;
|
41 | 1841 | djacobs | } |
42 | |||
43 | |||
44 | 1864 | azirbel | /**
|
45 | * Follows a line at the given speed.
|
||
46 | * @param speed The speed with which to follow the line.
|
||
47 | */
|
||
48 | 1851 | djacobs | int lineFollow(int speed) |
49 | 1841 | djacobs | { |
50 | 1851 | djacobs | int colors[5]; |
51 | 1841 | djacobs | int position;
|
52 | |||
53 | |||
54 | updateLine(colors); |
||
55 | position = lineLocate(colors); |
||
56 | |||
57 | //not on line
|
||
58 | if(position == NOLINE)
|
||
59 | { |
||
60 | 1931 | djacobs | if(lost++ > 20) |
61 | 1841 | djacobs | { |
62 | orb2_set_color(GREEN); |
||
63 | motors_off(); |
||
64 | return LINELOST;
|
||
65 | } |
||
66 | } |
||
67 | 1851 | djacobs | else if(position == FULL_LINE) |
68 | { |
||
69 | 1931 | djacobs | if(intersectionFilter++ > 4) |
70 | 1851 | djacobs | { |
71 | orb2_set_color(RED); |
||
72 | barCodePosition=0;
|
||
73 | 1853 | djacobs | disableBarCode=50;
|
74 | 1851 | djacobs | } |
75 | } |
||
76 | 1841 | djacobs | //on line
|
77 | else
|
||
78 | { |
||
79 | position*=30;
|
||
80 | orb2_set_color(ORB_OFF); |
||
81 | 1851 | djacobs | motorLeft(min(speed+position, 255));
|
82 | motorRight(min(speed-position, 255));
|
||
83 | 1841 | djacobs | lost=0;
|
84 | 1853 | djacobs | intersectionFilter=0;
|
85 | 1841 | djacobs | } |
86 | 1851 | djacobs | |
87 | 1853 | djacobs | if(disableBarCode--)
|
88 | 1851 | djacobs | { |
89 | 1853 | djacobs | if(disableBarCode)return NOBARCODE; |
90 | return INTERSECTION;
|
||
91 | 1851 | djacobs | } |
92 | 1842 | djacobs | updateBarCode(); |
93 | return getBarCode();
|
||
94 | 1841 | djacobs | } |
95 | |||
96 | 1851 | djacobs | |
97 | 1864 | azirbel | /**
|
98 | * Implements the left merge, assuming a line exists to the left. Works by
|
||
99 | * turning off the line at an increasing angle and waiting to hit another line
|
||
100 | * on the left.
|
||
101 | */
|
||
102 | 1854 | djacobs | int mergeLeft()
|
103 | 1851 | djacobs | { |
104 | 1854 | djacobs | motor_l_set(FORWARD, 200);
|
105 | if(turnDistance!=21)motor_r_set(FORWARD, 230); |
||
106 | else motor_r_set(FORWARD, 210); |
||
107 | 1851 | djacobs | int colors[5]; |
108 | updateLine(colors); |
||
109 | int position = lineLocate(colors);
|
||
110 | 1854 | djacobs | if(position>3 || position<-3)turnDistance++; |
111 | 1853 | djacobs | |
112 | 1854 | djacobs | if(turnDistance>20) |
113 | 1853 | djacobs | { |
114 | 1854 | djacobs | turnDistance=21;
|
115 | |||
116 | 1853 | djacobs | if(position<3 && position>-3) |
117 | { |
||
118 | turnDistance = 0;
|
||
119 | return 0; |
||
120 | } |
||
121 | } |
||
122 | 1851 | djacobs | return 1; |
123 | } |
||
124 | |||
125 | 1854 | djacobs | |
126 | 1864 | azirbel | /**
|
127 | * Implements the right merge, assuming a line exists to the right. Works by
|
||
128 | * turning off the line at an increasing angle and waiting to hit another line
|
||
129 | * on the right.
|
||
130 | */
|
||
131 | 1854 | djacobs | int mergeRight()
|
132 | { |
||
133 | motor_r_set(FORWARD, 200);
|
||
134 | if(turnDistance!=21)motor_l_set(FORWARD, 230); |
||
135 | else motor_l_set(FORWARD, 210); |
||
136 | int colors[5]; |
||
137 | updateLine(colors); |
||
138 | int position = lineLocate(colors);
|
||
139 | if(position>3 || position<-3)turnDistance++; |
||
140 | |||
141 | if(turnDistance>20) |
||
142 | { |
||
143 | turnDistance=21;
|
||
144 | |||
145 | if(position<3 && position>-3) |
||
146 | { |
||
147 | turnDistance = 0;
|
||
148 | return 0; |
||
149 | } |
||
150 | } |
||
151 | return 1; |
||
152 | } |
||
153 | |||
154 | |||
155 | |||
156 | 1864 | azirbel | /**
|
157 | * Turns left at a cross of two lines. Assumes that we are at lines in a cross
|
||
158 | * pattern, and turns until it sets straight on the new line.
|
||
159 | 1931 | djacobs | * @return 0 if turn finishes otherwise return 1
|
160 | 1864 | azirbel | */
|
161 | 1854 | djacobs | int turnLeft()
|
162 | { |
||
163 | 1931 | djacobs | /*motor_l_set(BACKWARD, 200);
|
164 | 1854 | djacobs | motor_r_set(FORWARD, 200);
|
165 | int colors[5];
|
||
166 | updateLine(colors);
|
||
167 | int position = lineLocate(colors);
|
||
168 | if(position>2 || position<-2)turnDistance++;
|
||
169 | if(turnDistance>1)
|
||
170 | {
|
||
171 | if(position<3 && position>-3)
|
||
172 | {
|
||
173 | turnDistance = 0;
|
||
174 | return 0;
|
||
175 | }
|
||
176 | }
|
||
177 | 1931 | djacobs | return 1;*/
|
178 | |||
179 | motor_l_set(BACKWARD,200);
|
||
180 | motor_r_set(FORWARD,200);
|
||
181 | if(!encoderReset)
|
||
182 | { |
||
183 | encoderStart = encoder_get_x(RIGHT); |
||
184 | encoderReset = 1;
|
||
185 | } |
||
186 | |||
187 | if(encoder_get_x(RIGHT) < encoderStart)
|
||
188 | { |
||
189 | encoderStart = 0;
|
||
190 | // Temporary: display an "error message" in case of overflow.
|
||
191 | // Using this for debugging, take it out soon!
|
||
192 | motor_l_set(FORWARD,0);
|
||
193 | motor_r_set(FORWARD,0);
|
||
194 | orb_set_color(WHITE); |
||
195 | delay_ms(2000);
|
||
196 | } |
||
197 | |||
198 | if(encoder_get_x(RIGHT) - encoderStart > 300) |
||
199 | { |
||
200 | encoderReset = 0;
|
||
201 | return 0; |
||
202 | } |
||
203 | 1854 | djacobs | return 1; |
204 | } |
||
205 | |||
206 | |||
207 | |||
208 | 1864 | azirbel | /**
|
209 | * Turns right at a cross of two lines. Assumes that we are at lines in a cross
|
||
210 | * pattern, and turns until it sets straight on the new line.
|
||
211 | 1931 | djacobs | * @return 0 if the turn finishes otherwise return 1
|
212 | 1864 | azirbel | */
|
213 | 1851 | djacobs | int turnRight()
|
214 | { |
||
215 | motor_r_set(BACKWARD, 200);
|
||
216 | motor_l_set(FORWARD, 200);
|
||
217 | int colors[5]; |
||
218 | updateLine(colors); |
||
219 | int position = lineLocate(colors);
|
||
220 | 1853 | djacobs | if(position>2 || position<-2)turnDistance++; |
221 | if(turnDistance>1) |
||
222 | { |
||
223 | if(position<3 && position>-3) |
||
224 | { |
||
225 | turnDistance = 0;
|
||
226 | return 0; |
||
227 | } |
||
228 | } |
||
229 | return 1; |
||
230 | 1851 | djacobs | } |
231 | |||
232 | |||
233 | |||
234 | int getBarCode()
|
||
235 | { |
||
236 | 1842 | djacobs | if(barCodePosition!=CODESIZE) return NOBARCODE ; |
237 | int temp = 0; |
||
238 | 1931 | djacobs | for(int i=0; i<CODESIZE; i++) |
239 | 1842 | djacobs | temp += (barCode[i] << i); |
240 | barCodePosition = 0;
|
||
241 | return temp;
|
||
242 | } |
||
243 | 1841 | djacobs | |
244 | |||
245 | |||
246 | void updateLine(int* values) |
||
247 | { |
||
248 | 1851 | djacobs | int ports[5] = {13, 12, 3, 2, 9}; |
249 | 1841 | djacobs | for(int i = 0; i<5; i++) |
250 | 1854 | djacobs | values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
|
251 | 1841 | djacobs | } |
252 | |||
253 | |||
254 | |||
255 | int lineLocate(int* colors) |
||
256 | { |
||
257 | int i;
|
||
258 | int wsum = 0; |
||
259 | int count=0; |
||
260 | |||
261 | for(i = 0; i<5; i++) |
||
262 | { |
||
263 | 1845 | dgurjar | count += colors[i]/2;
|
264 | 1841 | djacobs | wsum += (i)*colors[i]; |
265 | } |
||
266 | if(count==0) |
||
267 | 1851 | djacobs | return NOLINE;
|
268 | if(count==5) |
||
269 | return FULL_LINE;
|
||
270 | 1931 | djacobs | return (wsum/count)-4; // Subtract 4 to center the index around the center. |
271 | 1841 | djacobs | } |
272 | |||
273 | |||
274 | 1842 | djacobs | void updateBarCode()
|
275 | 1841 | djacobs | { |
276 | 1931 | djacobs | //! Note: currently only uses one of the barcode sensors.
|
277 | 1854 | djacobs | |
278 | 1841 | djacobs | //maps the sensors to the analog input ports
|
279 | int ports[2] = {8,1}; |
||
280 | 1842 | djacobs | int current[2]; |
281 | 1854 | djacobs | // current[0] = analog_get10(ports[0]);
|
282 | current[1] = analog_get10(ports[1]); |
||
283 | 1842 | djacobs | |
284 | if(current[1]>500) |
||
285 | { |
||
286 | if(countHi++==0) |
||
287 | { |
||
288 | avg = 500;
|
||
289 | maxAvg = 500;
|
||
290 | } |
||
291 | else
|
||
292 | { |
||
293 | avg = 3*avg + current[1]; |
||
294 | avg/=4;
|
||
295 | maxAvg = max(maxAvg, avg); |
||
296 | } |
||
297 | } |
||
298 | 1845 | dgurjar | else if(countHi>5) |
299 | 1842 | djacobs | { |
300 | if(countLo++>15) |
||
301 | { |
||
302 | countHi=countLo=0;
|
||
303 | 1845 | dgurjar | if(maxAvg>825)orb1_set_color(RED); |
304 | else orb1_set_color(BLUE);
|
||
305 | 1851 | djacobs | barCode[barCodePosition++] = maxAvg > 825 ? 1:0; |
306 | 1842 | djacobs | } |
307 | } |
||
308 | else countHi/=2; |
||
309 | if(countHi==0)countLo=0; |
||
310 | 1841 | djacobs | } |
311 | |||
312 | |||
313 | int min(int x, int y){return x>y ? y : x;} |
||
314 | 1842 | djacobs | int max(int x, int y){return x<y ? y : x;} |
315 | 1841 | djacobs | |
316 | void motorLeft(int speed){ |
||
317 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
318 | 1841 | djacobs | } |
319 | |||
320 | void motorRight(int speed){ |
||
321 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
322 | 1841 | djacobs | } |