Statistics
| Revision:

root / trunk / code / projects / linefollowing / lineFollow.c @ 1931

History | View | Annotate | Download (6.58 KB)

1
/**
2
 * @file lineFollow.c
3
 * @defgroup lineFollwing Line Following
4
 *
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
 * @author Dan Jacobs and the Colony Project
11
 * @date 11-1-2010
12
 */
13

    
14
#include "lineFollow.h"
15

    
16
//! The number of bits expected in a barcode
17
#define CODESIZE 5 
18

    
19
int countHi = 0;
20
int countLo = 0;
21
int maxAvg, avg;
22
int barCode[ CODESIZE ];
23
int barCodePosition=0;
24

    
25
int turnDistance=0;
26
//! Counts the number of full line readings before we determine an intersection
27
int intersectionFilter=0;
28
int disableBarCode=0;
29

    
30
//! 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

    
34

    
35
void lineFollow_init()
36
{
37
        analog_init(0);
38
        lost = 0;
39
        intersectionFilter=0;
40
        disableBarCode=0;
41
}
42

    
43

    
44
/** 
45
 * Follows a line at the given speed.
46
 * @param speed The speed with which to follow the line.
47
 */
48
int lineFollow(int speed)
49
{
50
        int colors[5];
51
        int position;
52
        
53

    
54
        updateLine(colors);
55
        position = lineLocate(colors);         
56
        
57
        //not on line
58
        if(position == NOLINE)
59
        {
60
                if(lost++ > 20)
61
                {
62
                        orb2_set_color(GREEN);
63
                        motors_off();
64
                        return LINELOST;
65
                }
66
        }
67
        else if(position == FULL_LINE)
68
        {
69
                if(intersectionFilter++ > 4)
70
                {
71
                        orb2_set_color(RED);
72
                        barCodePosition=0;
73
                        disableBarCode=50;
74
                }
75
        }
76
        //on line
77
        else
78
        {
79
                position*=30;
80
                orb2_set_color(ORB_OFF);
81
                motorLeft(min(speed+position, 255));
82
                motorRight(min(speed-position, 255));
83
                lost=0;
84
                intersectionFilter=0;
85
        }
86

    
87
        if(disableBarCode--)
88
        {
89
                if(disableBarCode)return NOBARCODE;
90
                return INTERSECTION;
91
        }
92
        updateBarCode();
93
        return getBarCode();
94
}
95

    
96

    
97
/**
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
int mergeLeft()
103
{
104
        motor_l_set(FORWARD, 200);
105
        if(turnDistance!=21)motor_r_set(FORWARD, 230);
106
        else motor_r_set(FORWARD, 210);
107
        int colors[5];
108
        updateLine(colors);
109
        int position = lineLocate(colors);
110
        if(position>3 || position<-3)turnDistance++;
111

    
112
        if(turnDistance>20)
113
        {
114
        turnDistance=21;
115
        
116
                if(position<3 && position>-3)
117
                {
118
                        turnDistance = 0;
119
                        return 0;
120
                }        
121
        }
122
        return 1;
123
}
124

    
125

    
126
/**
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
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
/**
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
 * @return 0 if turn finishes otherwise return 1
160
 */
161
int turnLeft()
162
{
163
        /*motor_l_set(BACKWARD, 200);
164
        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
        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
        return 1;
204
}
205

    
206

    
207

    
208
/**
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
 * @return 0 if the turn finishes otherwise return 1
212
 */
213
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
        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
}
231

    
232

    
233

    
234
int getBarCode()
235
{
236
        if(barCodePosition!=CODESIZE) return NOBARCODE ;
237
        int temp = 0;
238
        for(int i=0; i<CODESIZE; i++)
239
                temp += (barCode[i] << i);
240
        barCodePosition = 0;
241
        return temp;
242
}
243

    
244

    
245

    
246
void updateLine(int* values)
247
{        
248
        int ports[5] = {13, 12, 3, 2, 9};
249
        for(int i = 0; i<5; i++)
250
                values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
251
}
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
                count += colors[i]/2;
264
                wsum += (i)*colors[i];
265
        }
266
        if(count==0)
267
                return NOLINE;        
268
        if(count==5)
269
                return FULL_LINE;
270
        return (wsum/count)-4; // Subtract 4 to center the index around the center.
271
}
272

    
273

    
274
void updateBarCode()
275
{
276
        //! Note: currently only uses one of the barcode sensors.
277

    
278
        //maps the sensors to the analog input ports
279
        int ports[2] = {8,1};
280
        int current[2];
281
//        current[0] = analog_get10(ports[0]);
282
        current[1] = analog_get10(ports[1]);
283

    
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
        else if(countHi>5)
299
        {
300
                if(countLo++>15)
301
                {
302
                        countHi=countLo=0;
303
                        if(maxAvg>825)orb1_set_color(RED);
304
                        else orb1_set_color(BLUE);
305
                        barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
306
                }
307
        }
308
        else countHi/=2;
309
        if(countHi==0)countLo=0; 
310
}
311

    
312

    
313
int min(int x, int y){return x>y ? y : x;}
314
int max(int x, int y){return x<y ? y : x;}
315

    
316
void motorLeft(int speed){
317
        ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);
318
}
319

    
320
void motorRight(int speed){
321
        ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127);
322
}