root / trunk / code / projects / linefollowing / lineFollow.c @ 1878
History | View | Annotate | Download (5.71 KB)
1 | 1864 | azirbel | /**
|
---|---|---|---|
2 | * @file lineFollow.c
|
||
3 | *
|
||
4 | * Takes care of following a line. Running this program is done by calling the
|
||
5 | * init() function and then the lineFollow(speed) command. However, direct use
|
||
6 | * of this class is discouraged as its behavior is used by lineDrive.c, which
|
||
7 | * extends this class to provide behavior functionality.
|
||
8 | *
|
||
9 | * @author Dan Jacobs
|
||
10 | * @date 11-1-2010
|
||
11 | */
|
||
12 | |||
13 | 1841 | djacobs | #include "lineFollow.h" |
14 | |||
15 | 1845 | dgurjar | #define CODESIZE 5 |
16 | 1841 | djacobs | |
17 | 1842 | djacobs | int countHi = 0; |
18 | int countLo = 0; |
||
19 | int maxAvg, avg;
|
||
20 | int barCode[ CODESIZE ];
|
||
21 | int barCodePosition=0; |
||
22 | 1841 | djacobs | |
23 | 1851 | djacobs | int turnDistance=0; |
24 | 1853 | djacobs | int intersectionFilter=0; |
25 | 1851 | djacobs | int disableBarCode=0; |
26 | |||
27 | |||
28 | 1864 | azirbel | /**
|
29 | * Initializes line following. Must be called before other line-following
|
||
30 | * behavior will work.
|
||
31 | */
|
||
32 | 1841 | djacobs | void lineFollow_init()
|
33 | { |
||
34 | analog_init(0);
|
||
35 | lost = 0;
|
||
36 | 1853 | djacobs | intersectionFilter=0;
|
37 | 1851 | djacobs | disableBarCode=0;
|
38 | 1841 | djacobs | } |
39 | |||
40 | |||
41 | 1864 | azirbel | /**
|
42 | * Follows a line at the given speed.
|
||
43 | * @param speed The speed with which to follow the line.
|
||
44 | */
|
||
45 | 1851 | djacobs | int lineFollow(int speed) |
46 | 1841 | djacobs | { |
47 | 1851 | djacobs | int colors[5]; |
48 | 1841 | djacobs | int position;
|
49 | |||
50 | |||
51 | updateLine(colors); |
||
52 | position = lineLocate(colors); |
||
53 | |||
54 | //not on line
|
||
55 | if(position == NOLINE)
|
||
56 | { |
||
57 | if(lost++>20) |
||
58 | { |
||
59 | orb2_set_color(GREEN); |
||
60 | motors_off(); |
||
61 | return LINELOST;
|
||
62 | } |
||
63 | } |
||
64 | 1851 | djacobs | else if(position == FULL_LINE) |
65 | { |
||
66 | 1853 | djacobs | if(intersectionFilter++>4) |
67 | 1851 | djacobs | { |
68 | orb2_set_color(RED); |
||
69 | barCodePosition=0;
|
||
70 | 1853 | djacobs | disableBarCode=50;
|
71 | 1851 | djacobs | } |
72 | } |
||
73 | 1841 | djacobs | //on line
|
74 | else
|
||
75 | { |
||
76 | position*=30;
|
||
77 | orb2_set_color(ORB_OFF); |
||
78 | 1851 | djacobs | motorLeft(min(speed+position, 255));
|
79 | motorRight(min(speed-position, 255));
|
||
80 | 1841 | djacobs | lost=0;
|
81 | 1853 | djacobs | intersectionFilter=0;
|
82 | 1841 | djacobs | } |
83 | 1851 | djacobs | |
84 | 1853 | djacobs | if(disableBarCode--)
|
85 | 1851 | djacobs | { |
86 | 1853 | djacobs | if(disableBarCode)return NOBARCODE; |
87 | return INTERSECTION;
|
||
88 | 1851 | djacobs | } |
89 | 1842 | djacobs | updateBarCode(); |
90 | return getBarCode();
|
||
91 | 1841 | djacobs | } |
92 | |||
93 | 1851 | djacobs | |
94 | 1864 | azirbel | /**
|
95 | * Implements the left merge, assuming a line exists to the left. Works by
|
||
96 | * turning off the line at an increasing angle and waiting to hit another line
|
||
97 | * on the left.
|
||
98 | */
|
||
99 | 1854 | djacobs | int mergeLeft()
|
100 | 1851 | djacobs | { |
101 | 1854 | djacobs | motor_l_set(FORWARD, 200);
|
102 | if(turnDistance!=21)motor_r_set(FORWARD, 230); |
||
103 | else motor_r_set(FORWARD, 210); |
||
104 | 1851 | djacobs | int colors[5]; |
105 | updateLine(colors); |
||
106 | int position = lineLocate(colors);
|
||
107 | 1854 | djacobs | if(position>3 || position<-3)turnDistance++; |
108 | 1853 | djacobs | |
109 | 1854 | djacobs | if(turnDistance>20) |
110 | 1853 | djacobs | { |
111 | 1854 | djacobs | turnDistance=21;
|
112 | |||
113 | 1853 | djacobs | if(position<3 && position>-3) |
114 | { |
||
115 | turnDistance = 0;
|
||
116 | return 0; |
||
117 | } |
||
118 | } |
||
119 | 1851 | djacobs | return 1; |
120 | } |
||
121 | |||
122 | 1854 | djacobs | |
123 | 1864 | azirbel | /**
|
124 | * Implements the right merge, assuming a line exists to the right. Works by
|
||
125 | * turning off the line at an increasing angle and waiting to hit another line
|
||
126 | * on the right.
|
||
127 | */
|
||
128 | 1854 | djacobs | int mergeRight()
|
129 | { |
||
130 | motor_r_set(FORWARD, 200);
|
||
131 | if(turnDistance!=21)motor_l_set(FORWARD, 230); |
||
132 | else motor_l_set(FORWARD, 210); |
||
133 | int colors[5]; |
||
134 | updateLine(colors); |
||
135 | int position = lineLocate(colors);
|
||
136 | if(position>3 || position<-3)turnDistance++; |
||
137 | |||
138 | if(turnDistance>20) |
||
139 | { |
||
140 | turnDistance=21;
|
||
141 | |||
142 | if(position<3 && position>-3) |
||
143 | { |
||
144 | turnDistance = 0;
|
||
145 | return 0; |
||
146 | } |
||
147 | } |
||
148 | return 1; |
||
149 | } |
||
150 | |||
151 | |||
152 | |||
153 | 1864 | azirbel | /**
|
154 | * Turns left at a cross of two lines. Assumes that we are at lines in a cross
|
||
155 | * pattern, and turns until it sets straight on the new line.
|
||
156 | 1878 | dgurjar | * @return 0 if turn finishes otherwise return 1
|
157 | 1864 | azirbel | */
|
158 | 1854 | djacobs | int turnLeft()
|
159 | { |
||
160 | motor_l_set(BACKWARD, 200);
|
||
161 | motor_r_set(FORWARD, 200);
|
||
162 | int colors[5]; |
||
163 | updateLine(colors); |
||
164 | int position = lineLocate(colors);
|
||
165 | if(position>2 || position<-2)turnDistance++; |
||
166 | if(turnDistance>1) |
||
167 | { |
||
168 | if(position<3 && position>-3) |
||
169 | { |
||
170 | turnDistance = 0;
|
||
171 | return 0; |
||
172 | } |
||
173 | } |
||
174 | return 1; |
||
175 | } |
||
176 | |||
177 | |||
178 | |||
179 | 1864 | azirbel | /**
|
180 | * Turns right at a cross of two lines. Assumes that we are at lines in a cross
|
||
181 | * pattern, and turns until it sets straight on the new line.
|
||
182 | 1878 | dgurjar | * @return 0 if the turn finishes otherwise return 1
|
183 | 1864 | azirbel | */
|
184 | 1851 | djacobs | int turnRight()
|
185 | { |
||
186 | motor_r_set(BACKWARD, 200);
|
||
187 | motor_l_set(FORWARD, 200);
|
||
188 | int colors[5]; |
||
189 | updateLine(colors); |
||
190 | int position = lineLocate(colors);
|
||
191 | 1853 | djacobs | if(position>2 || position<-2)turnDistance++; |
192 | if(turnDistance>1) |
||
193 | { |
||
194 | if(position<3 && position>-3) |
||
195 | { |
||
196 | turnDistance = 0;
|
||
197 | return 0; |
||
198 | } |
||
199 | } |
||
200 | return 1; |
||
201 | 1851 | djacobs | } |
202 | |||
203 | |||
204 | |||
205 | |||
206 | int getBarCode()
|
||
207 | { |
||
208 | 1842 | djacobs | if(barCodePosition!=CODESIZE) return NOBARCODE ; |
209 | int temp = 0; |
||
210 | int i;
|
||
211 | for(i=0; i<CODESIZE; i++) |
||
212 | temp += (barCode[i] << i); |
||
213 | barCodePosition = 0;
|
||
214 | return temp;
|
||
215 | } |
||
216 | 1841 | djacobs | |
217 | |||
218 | |||
219 | void updateLine(int* values) |
||
220 | { |
||
221 | 1851 | djacobs | int ports[5] = {13, 12, 3, 2, 9}; |
222 | 1841 | djacobs | for(int i = 0; i<5; i++) |
223 | 1854 | djacobs | values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
|
224 | 1841 | djacobs | } |
225 | |||
226 | |||
227 | |||
228 | int lineLocate(int* colors) |
||
229 | { |
||
230 | int i;
|
||
231 | int wsum = 0; |
||
232 | int count=0; |
||
233 | |||
234 | for(i = 0; i<5; i++) |
||
235 | { |
||
236 | 1845 | dgurjar | count += colors[i]/2;
|
237 | 1841 | djacobs | wsum += (i)*colors[i]; |
238 | } |
||
239 | if(count==0) |
||
240 | 1851 | djacobs | return NOLINE;
|
241 | if(count==5) |
||
242 | return FULL_LINE;
|
||
243 | 1841 | djacobs | return (wsum/count)-4; |
244 | } |
||
245 | |||
246 | |||
247 | 1842 | djacobs | void updateBarCode()
|
248 | 1841 | djacobs | { |
249 | 1854 | djacobs | |
250 | //NOTE: currently only uses one of the barcode sensors.
|
||
251 | |||
252 | 1841 | djacobs | //maps the sensors to the analog input ports
|
253 | int ports[2] = {8,1}; |
||
254 | 1842 | djacobs | int current[2]; |
255 | 1854 | djacobs | // current[0] = analog_get10(ports[0]);
|
256 | current[1] = analog_get10(ports[1]); |
||
257 | 1842 | djacobs | |
258 | if(current[1]>500) |
||
259 | { |
||
260 | if(countHi++==0) |
||
261 | { |
||
262 | avg = 500;
|
||
263 | maxAvg = 500;
|
||
264 | } |
||
265 | else
|
||
266 | { |
||
267 | avg = 3*avg + current[1]; |
||
268 | avg/=4;
|
||
269 | maxAvg = max(maxAvg, avg); |
||
270 | } |
||
271 | } |
||
272 | 1845 | dgurjar | else if(countHi>5) |
273 | 1842 | djacobs | { |
274 | if(countLo++>15) |
||
275 | { |
||
276 | countHi=countLo=0;
|
||
277 | 1845 | dgurjar | if(maxAvg>825)orb1_set_color(RED); |
278 | else orb1_set_color(BLUE);
|
||
279 | 1851 | djacobs | barCode[barCodePosition++] = maxAvg > 825 ? 1:0; |
280 | 1842 | djacobs | } |
281 | } |
||
282 | else countHi/=2; |
||
283 | if(countHi==0)countLo=0; |
||
284 | 1841 | djacobs | } |
285 | |||
286 | |||
287 | 1864 | azirbel | //! A simple function to return the minimum of two integers.
|
288 | 1841 | djacobs | int min(int x, int y){return x>y ? y : x;} |
289 | 1864 | azirbel | //! A simple function to return the maximum of two integers.
|
290 | 1842 | djacobs | int max(int x, int y){return x<y ? y : x;} |
291 | 1841 | djacobs | |
292 | void motorLeft(int speed){ |
||
293 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
294 | 1841 | djacobs | } |
295 | |||
296 | void motorRight(int speed){ |
||
297 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
298 | 1841 | djacobs | } |