scoutos / scout / libscout / src / behaviors / line_follow.cpp @ 60b98383
History | View | Annotate | Download (5.57 KB)
1 | 9143e077 | Lalitha Ganesan | /**
|
---|---|---|---|
2 | 60b98383 | Priya | * @file line_follow.cpp
|
3 | 9143e077 | Lalitha Ganesan | * @defgroup lineFollwing Line Following
|
4 | *
|
||
5 | * Takes care of following a line. Running this program is done by calling the
|
||
6 | 60b98383 | Priya | * init() function and then the line_follow(speed) command. However, direct use
|
7 | 9143e077 | Lalitha Ganesan | * of this class is discouraged as its behavior is used by lineDrive.cpp, which
|
8 | * extends this class to provide behavior functionality.
|
||
9 | *
|
||
10 | * @author Dan Jacobs and the Colony Project
|
||
11 | * @date 11-1-2010
|
||
12 | */
|
||
13 | |||
14 | 60b98383 | Priya | #include "line_follow.h" |
15 | 9143e077 | Lalitha Ganesan | |
16 | //! Anything lower than this value is white
|
||
17 | int GREY_THRESHOLD = 300; |
||
18 | //! Anything higher than this value is black
|
||
19 | int BLACK_THRESHOLD = 750; |
||
20 | |||
21 | int countHi = 0; |
||
22 | int countLo = 0; |
||
23 | int maxAvg, avg;
|
||
24 | |||
25 | int duration[2] = {0}; |
||
26 | int lastColor[2] = {0}; |
||
27 | char isReset[2] = {1}; |
||
28 | int lastReadings[2][ NUM_READINGS ] = {{0}}; |
||
29 | int lastReadingsPtr[2] = {0}; |
||
30 | int numLast[2][4] = { {0, 0, 0, NUM_READINGS}, {0, 0, 0, NUM_READINGS} }; |
||
31 | int bitColor[2] = {0}; |
||
32 | |||
33 | int turnDistance=0; |
||
34 | //! Counts the number of full line readings before we determine an intersection
|
||
35 | int intersectionFilter=0; |
||
36 | |||
37 | //! Keeps track of where the encoder of one motor started, for use in turns.
|
||
38 | int encoderStart = -1; |
||
39 | int encoderReset = 0; // 0 if encoderStart has no value set |
||
40 | |||
41 | 60b98383 | Priya | int min(int x, int y){return x>y ? y : x;} |
42 | int max(int x, int y){return x<y ? y : x;} |
||
43 | 9143e077 | Lalitha Ganesan | |
44 | |||
45 | 60b98383 | Priya | void line_follow::init()
|
46 | { |
||
47 | int i, j, curReading;
|
||
48 | int lowGrey = 1000, highGrey = 0, lowBlack = 1000, |
||
49 | highBlack = 0;
|
||
50 | |||
51 | analog_init(0);
|
||
52 | encoders_init(); |
||
53 | lost = 0;
|
||
54 | intersectionFilter=0;
|
||
55 | |||
56 | for(i=0; i<2; i++) |
||
57 | { |
||
58 | for(j=0; j<NUM_READINGS; j++) |
||
59 | 9143e077 | Lalitha Ganesan | { |
60 | 60b98383 | Priya | lastReadings[i][j] = BAD_READING; |
61 | 9143e077 | Lalitha Ganesan | } |
62 | 60b98383 | Priya | isReset[i] = 1;
|
63 | } |
||
64 | |||
65 | // Calibrate thresholds
|
||
66 | orb_set_color(YELLOW); |
||
67 | delay_ms(2000);
|
||
68 | |||
69 | orb_set_color(BLUE); |
||
70 | |||
71 | for(i=0; i<100; i++) |
||
72 | { |
||
73 | curReading = read_line(LEFT_SENSOR + 6);
|
||
74 | if(curReading < lowGrey)
|
||
75 | lowGrey = curReading; |
||
76 | if(curReading > highGrey)
|
||
77 | highGrey = curReading; |
||
78 | |||
79 | delay_ms(20);
|
||
80 | } |
||
81 | |||
82 | orb_set_color(YELLOW); |
||
83 | delay_ms(2000);
|
||
84 | |||
85 | orb_set_color(GREEN); |
||
86 | |||
87 | for(i=0; i<100; i++) |
||
88 | { |
||
89 | curReading = read_line(LEFT_SENSOR + 6);
|
||
90 | if(curReading < lowBlack)
|
||
91 | lowBlack = curReading; |
||
92 | if(curReading > highBlack)
|
||
93 | highBlack = curReading; |
||
94 | |||
95 | delay_ms(20);
|
||
96 | } |
||
97 | |||
98 | orbs_set(0,0,0,0,0,0); |
||
99 | |||
100 | GREY_THRESHOLD = lowGrey / 2;
|
||
101 | BLACK_THRESHOLD = (highGrey + lowBlack) / 2;
|
||
102 | |||
103 | usb_puts("Grey: ");
|
||
104 | usb_puti(lowGrey); |
||
105 | usb_puts(", ");
|
||
106 | usb_puti(highGrey); |
||
107 | usb_puts("\nBlack: ");
|
||
108 | usb_puti(lowBlack); |
||
109 | usb_puts(", ");
|
||
110 | usb_puti(highBlack); |
||
111 | usb_puts("\nThresholds: ");
|
||
112 | usb_puti(GREY_THRESHOLD); |
||
113 | usb_puts(", ");
|
||
114 | usb_puti(BLACK_THRESHOLD); |
||
115 | usb_puts("\n\n");
|
||
116 | |||
117 | delay_ms(1500);
|
||
118 | 9143e077 | Lalitha Ganesan | } |
119 | |||
120 | |||
121 | /**
|
||
122 | * Follows a line at the given speed.
|
||
123 | * @param speed The speed with which to follow the line.
|
||
124 | */
|
||
125 | 60b98383 | Priya | int line_follow::follow_line(int speed) |
126 | 9143e077 | Lalitha Ganesan | { |
127 | int colors[5]; |
||
128 | int position;
|
||
129 | |||
130 | |||
131 | updateLine(colors); |
||
132 | position = lineLocate(colors); |
||
133 | |||
134 | //not on line
|
||
135 | if(position == NOLINE)
|
||
136 | { |
||
137 | if(lost++ > 20) |
||
138 | { |
||
139 | orb2_set_color(GREEN); |
||
140 | motors_off(); |
||
141 | return LINELOST;
|
||
142 | } |
||
143 | } |
||
144 | else if(position == FULL_LINE) |
||
145 | { |
||
146 | if(intersectionFilter++ > 4) |
||
147 | { |
||
148 | orb2_set_color(RED); |
||
149 | barCodePosition[0]=0; |
||
150 | barCodePosition[1]=0; |
||
151 | disableBarCode=50;
|
||
152 | } |
||
153 | } |
||
154 | //on line
|
||
155 | else
|
||
156 | { |
||
157 | position*=30;
|
||
158 | orb2_set_color(ORB_OFF); |
||
159 | motorLeft(min(speed+position, 255));
|
||
160 | motorRight(min(speed-position, 255));
|
||
161 | lost=0;
|
||
162 | intersectionFilter=0;
|
||
163 | } |
||
164 | } |
||
165 | |||
166 | |||
167 | /**
|
||
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 | * @return 0 if turn finishes otherwise return 1
|
||
171 | */
|
||
172 | 60b98383 | Priya | int line_follow::turnLeft()
|
173 | 9143e077 | Lalitha Ganesan | { |
174 | motors->set_sides(-20, 20, MOTOR_ABSOLUTE); |
||
175 | |||
176 | if(!encoderReset)
|
||
177 | { |
||
178 | encoderStart = encoder_get_x(RIGHT); |
||
179 | encoderReset = 1;
|
||
180 | } |
||
181 | |||
182 | if(encoder_get_x(RIGHT) < encoderStart)
|
||
183 | { |
||
184 | encoderStart = 0;
|
||
185 | motors->set_sides(0, 0, MOTOR_ABSOLUTE); |
||
186 | delay_ms(2000);
|
||
187 | } |
||
188 | |||
189 | if(encoder_get_x(RIGHT) - encoderStart > 300) |
||
190 | { |
||
191 | encoderReset = 0;
|
||
192 | return 0; |
||
193 | } |
||
194 | return 1; |
||
195 | } |
||
196 | |||
197 | |||
198 | |||
199 | /**
|
||
200 | * Turns right at a cross of two lines. Assumes that we are at lines in a cross
|
||
201 | * pattern, and turns until it sets straight on the new line.
|
||
202 | * @return 0 if the turn finishes otherwise return 1
|
||
203 | */
|
||
204 | 60b98383 | Priya | int line_follow::turnRight()
|
205 | 9143e077 | Lalitha Ganesan | { |
206 | 60b98383 | Priya | /// Set motors using new motors functions
|
207 | motors->set_sides(20, -20, MOTOR_ABSOLUTE); |
||
208 | |||
209 | int colors[5]; |
||
210 | updateLine(colors); |
||
211 | int position = lineLocate(colors);
|
||
212 | if(position>2 || position<-2) |
||
213 | turnDistance++; |
||
214 | |||
215 | if(turnDistance>1) |
||
216 | { |
||
217 | if(position<3 && position>-3) |
||
218 | { |
||
219 | turnDistance = 0;
|
||
220 | return 0; |
||
221 | } |
||
222 | } |
||
223 | return 1; |
||
224 | 9143e077 | Lalitha Ganesan | } |
225 | |||
226 | |||
227 | 60b98383 | Priya | void line_follow::updateLine(int* values) |
228 | 9143e077 | Lalitha Ganesan | { |
229 | for(int i = 0; i<5; i++) |
||
230 | values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
|
||
231 | } |
||
232 | |||
233 | |||
234 | 60b98383 | Priya | int line_follow::lineLocate(int* colors) |
235 | 9143e077 | Lalitha Ganesan | { |
236 | int i;
|
||
237 | int wsum = 0; |
||
238 | int count=0; |
||
239 | |||
240 | for(i = 0; i<5; i++) |
||
241 | { |
||
242 | count += colors[i]/2;
|
||
243 | wsum += (i)*colors[i]; |
||
244 | } |
||
245 | if(count==0) |
||
246 | return NOLINE;
|
||
247 | if(count==5) |
||
248 | return FULL_LINE;
|
||
249 | return (wsum/count)-4; /// Subtract 4 to center the index around the center. |
||
250 | } |
||
251 | |||
252 | 60b98383 | Priya | void line_follow::motorLeft(int speed) |
253 | 9143e077 | Lalitha Ganesan | { |
254 | int tempspeed = 0; |
||
255 | if ((speed-=127)>=0) |
||
256 | { |
||
257 | tempspeed = 160+speed*95/128; |
||
258 | 60b98383 | Priya | motors->set(MOTOR_LEFT, tempspeed, MOTOR_ABSOLUTE); |
259 | 9143e077 | Lalitha Ganesan | } |
260 | else
|
||
261 | { |
||
262 | tempspeed = 160+speed*95/128; |
||
263 | 60b98383 | Priya | motors->set(MOTOR_LEFT, -tempspeed, MOTOR_ABSOLUTE); |
264 | 9143e077 | Lalitha Ganesan | } |
265 | } |
||
266 | |||
267 | 60b98383 | Priya | void line_follow::motorRight(int speed) |
268 | 9143e077 | Lalitha Ganesan | { |
269 | int tempspeed = 0; |
||
270 | if ((speed-=127)>=0) |
||
271 | { |
||
272 | tempspeed = 160+speed*95/128; |
||
273 | 60b98383 | Priya | motors->set(MOTOR_RIGHT, tempspeed, MOTOR_ABSOLUTE); |
274 | 9143e077 | Lalitha Ganesan | } |
275 | else
|
||
276 | { |
||
277 | tempspeed = 160+speed*95/128; |
||
278 | 60b98383 | Priya | motors->set(MOTOR_RIGHT, -tempspeed, MOTOR_ABSOLUTE); |
279 | 9143e077 | Lalitha Ganesan | } |
280 | } |
||
281 | aa84b67c | James Carroll |