root / trunk / code / projects / traffic_navigation / lineFollow.c @ 1858
History | View | Annotate | Download (4.28 KB)
1 | 1858 | mjmckay | #include "lineFollow.h" |
---|---|---|---|
2 | 1846 | bwasserm | |
3 | #define CODESIZE 5 |
||
4 | |||
5 | int countHi = 0; |
||
6 | int countLo = 0; |
||
7 | int maxAvg, avg;
|
||
8 | int barCode[ CODESIZE ];
|
||
9 | int barCodePosition=0; |
||
10 | |||
11 | 1858 | mjmckay | int turnDistance=0; |
12 | int intersectionFilter=0; |
||
13 | int disableBarCode=0; |
||
14 | |||
15 | |||
16 | 1846 | bwasserm | void lineFollow_init()
|
17 | { |
||
18 | analog_init(0);
|
||
19 | lost = 0;
|
||
20 | 1858 | mjmckay | intersectionFilter=0;
|
21 | disableBarCode=0;
|
||
22 | 1846 | bwasserm | } |
23 | |||
24 | |||
25 | |||
26 | 1858 | mjmckay | int lineFollow(int speed) |
27 | 1846 | bwasserm | { |
28 | 1858 | mjmckay | int colors[5]; |
29 | 1846 | bwasserm | int position;
|
30 | |||
31 | |||
32 | updateLine(colors); |
||
33 | position = lineLocate(colors); |
||
34 | |||
35 | //not on line
|
||
36 | if(position == NOLINE)
|
||
37 | { |
||
38 | if(lost++>20) |
||
39 | { |
||
40 | orb2_set_color(GREEN); |
||
41 | motors_off(); |
||
42 | return LINELOST;
|
||
43 | } |
||
44 | } |
||
45 | 1858 | mjmckay | else if(position == FULL_LINE) |
46 | { |
||
47 | if(intersectionFilter++>4) |
||
48 | { |
||
49 | orb2_set_color(RED); |
||
50 | barCodePosition=0;
|
||
51 | disableBarCode=50;
|
||
52 | } |
||
53 | } |
||
54 | 1846 | bwasserm | //on line
|
55 | else
|
||
56 | { |
||
57 | position*=30;
|
||
58 | orb2_set_color(ORB_OFF); |
||
59 | 1858 | mjmckay | motorLeft(min(speed+position, 255));
|
60 | motorRight(min(speed-position, 255));
|
||
61 | 1846 | bwasserm | lost=0;
|
62 | 1858 | mjmckay | intersectionFilter=0;
|
63 | 1846 | bwasserm | } |
64 | 1858 | mjmckay | |
65 | if(disableBarCode--)
|
||
66 | { |
||
67 | if(disableBarCode)return NOBARCODE; |
||
68 | return INTERSECTION;
|
||
69 | } |
||
70 | 1846 | bwasserm | updateBarCode(); |
71 | return getBarCode();
|
||
72 | } |
||
73 | |||
74 | 1858 | mjmckay | |
75 | int mergeLeft()
|
||
76 | { |
||
77 | motor_l_set(FORWARD, 200);
|
||
78 | if(turnDistance!=21)motor_r_set(FORWARD, 230); |
||
79 | else motor_r_set(FORWARD, 210); |
||
80 | int colors[5]; |
||
81 | updateLine(colors); |
||
82 | int position = lineLocate(colors);
|
||
83 | if(position>3 || position<-3)turnDistance++; |
||
84 | |||
85 | if(turnDistance>20) |
||
86 | { |
||
87 | turnDistance=21;
|
||
88 | |||
89 | if(position<3 && position>-3) |
||
90 | { |
||
91 | turnDistance = 0;
|
||
92 | return 0; |
||
93 | } |
||
94 | } |
||
95 | return 1; |
||
96 | } |
||
97 | |||
98 | |||
99 | int mergeRight()
|
||
100 | { |
||
101 | motor_r_set(FORWARD, 200);
|
||
102 | if(turnDistance!=21)motor_l_set(FORWARD, 230); |
||
103 | else motor_l_set(FORWARD, 210); |
||
104 | int colors[5]; |
||
105 | updateLine(colors); |
||
106 | int position = lineLocate(colors);
|
||
107 | if(position>3 || position<-3)turnDistance++; |
||
108 | |||
109 | if(turnDistance>20) |
||
110 | { |
||
111 | turnDistance=21;
|
||
112 | |||
113 | if(position<3 && position>-3) |
||
114 | { |
||
115 | turnDistance = 0;
|
||
116 | return 0; |
||
117 | } |
||
118 | } |
||
119 | return 1; |
||
120 | } |
||
121 | |||
122 | |||
123 | |||
124 | int turnLeft()
|
||
125 | { |
||
126 | motor_l_set(BACKWARD, 200);
|
||
127 | motor_r_set(FORWARD, 200);
|
||
128 | int colors[5]; |
||
129 | updateLine(colors); |
||
130 | int position = lineLocate(colors);
|
||
131 | if(position>2 || position<-2)turnDistance++; |
||
132 | if(turnDistance>1) |
||
133 | { |
||
134 | if(position<3 && position>-3) |
||
135 | { |
||
136 | turnDistance = 0;
|
||
137 | return 0; |
||
138 | } |
||
139 | } |
||
140 | return 1; |
||
141 | } |
||
142 | |||
143 | |||
144 | |||
145 | int turnRight()
|
||
146 | { |
||
147 | motor_r_set(BACKWARD, 200);
|
||
148 | motor_l_set(FORWARD, 200);
|
||
149 | int colors[5]; |
||
150 | updateLine(colors); |
||
151 | int position = lineLocate(colors);
|
||
152 | if(position>2 || position<-2)turnDistance++; |
||
153 | if(turnDistance>1) |
||
154 | { |
||
155 | if(position<3 && position>-3) |
||
156 | { |
||
157 | turnDistance = 0;
|
||
158 | return 0; |
||
159 | } |
||
160 | } |
||
161 | return 1; |
||
162 | } |
||
163 | |||
164 | |||
165 | |||
166 | |||
167 | int getBarCode()
|
||
168 | { |
||
169 | 1846 | bwasserm | if(barCodePosition!=CODESIZE) return NOBARCODE ; |
170 | int temp = 0; |
||
171 | int i;
|
||
172 | for(i=0; i<CODESIZE; i++) |
||
173 | temp += (barCode[i] << i); |
||
174 | barCodePosition = 0;
|
||
175 | return temp;
|
||
176 | } |
||
177 | |||
178 | |||
179 | |||
180 | void updateLine(int* values) |
||
181 | { |
||
182 | 1858 | mjmckay | int ports[5] = {13, 12, 3, 2, 9}; |
183 | 1846 | bwasserm | for(int i = 0; i<5; i++) |
184 | 1858 | mjmckay | values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
|
185 | 1846 | bwasserm | } |
186 | |||
187 | |||
188 | |||
189 | int lineLocate(int* colors) |
||
190 | { |
||
191 | int i;
|
||
192 | int wsum = 0; |
||
193 | int count=0; |
||
194 | |||
195 | for(i = 0; i<5; i++) |
||
196 | { |
||
197 | count += colors[i]/2;
|
||
198 | wsum += (i)*colors[i]; |
||
199 | } |
||
200 | if(count==0) |
||
201 | 1858 | mjmckay | return NOLINE;
|
202 | if(count==5) |
||
203 | return FULL_LINE;
|
||
204 | 1846 | bwasserm | return (wsum/count)-4; |
205 | } |
||
206 | |||
207 | |||
208 | void updateBarCode()
|
||
209 | { |
||
210 | 1858 | mjmckay | |
211 | //NOTE: currently only uses one of the barcode sensors.
|
||
212 | |||
213 | 1846 | bwasserm | //maps the sensors to the analog input ports
|
214 | int ports[2] = {8,1}; |
||
215 | int current[2]; |
||
216 | 1858 | mjmckay | // current[0] = analog_get10(ports[0]);
|
217 | current[1] = analog_get10(ports[1]); |
||
218 | 1846 | bwasserm | |
219 | if(current[1]>500) |
||
220 | { |
||
221 | if(countHi++==0) |
||
222 | { |
||
223 | avg = 500;
|
||
224 | maxAvg = 500;
|
||
225 | } |
||
226 | else
|
||
227 | { |
||
228 | avg = 3*avg + current[1]; |
||
229 | avg/=4;
|
||
230 | maxAvg = max(maxAvg, avg); |
||
231 | } |
||
232 | } |
||
233 | else if(countHi>5) |
||
234 | { |
||
235 | if(countLo++>15) |
||
236 | { |
||
237 | countHi=countLo=0;
|
||
238 | if(maxAvg>825)orb1_set_color(RED); |
||
239 | else orb1_set_color(BLUE);
|
||
240 | 1858 | mjmckay | barCode[barCodePosition++] = maxAvg > 825 ? 1:0; |
241 | 1846 | bwasserm | } |
242 | } |
||
243 | else countHi/=2; |
||
244 | if(countHi==0)countLo=0; |
||
245 | } |
||
246 | |||
247 | |||
248 | int min(int x, int y){return x>y ? y : x;} |
||
249 | int max(int x, int y){return x<y ? y : x;} |
||
250 | |||
251 | void motorLeft(int speed){ |
||
252 | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
||
253 | } |
||
254 | |||
255 | void motorRight(int speed){ |
||
256 | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
||
257 | } |