Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (8.67 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
#define LINE_COLOR 200
19

    
20

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

    
27
int duration = 0;
28
int lastColor = 0;
29
char isReset = 0;
30

    
31
int turnDistance=0;
32
//! Counts the number of full line readings before we determine an intersection
33
int intersectionFilter=0;
34
int disableBarCode=0;
35

    
36
//! Keeps track of where the encoder of one motor started, for use in turns.
37
int encoderStart = -1;
38
int encoderReset = 0;   // 0 if encoderStart has no value set
39

    
40

    
41
void lineFollow_init()
42
{
43
        analog_init(0);
44
        lost = 0;
45
        intersectionFilter=0;
46
        disableBarCode=0;
47
}
48

    
49

    
50
/** 
51
 * Follows a line at the given speed.
52
 * @param speed The speed with which to follow the line.
53
 */
54
int lineFollow(int speed)
55
{
56
        int colors[5];
57
        int position;
58
        
59

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

    
93
    // If we're running over a line, stop reading barcodes for a sec
94
        if(disableBarCode-- > 0)
95
        {
96
        // Return intersection once we cross the line
97
                if(disableBarCode) return NOBARCODE;
98
                return INTERSECTION;
99
        }
100
        updateBarCode();
101
        return getBarCode();
102
}
103

    
104

    
105
/**
106
 * Implements the left merge, assuming a line exists to the left.  Works by
107
 * turning off the line at an increasing angle and waiting to hit another line
108
 * on the left.
109
 */
110
int mergeLeft()
111
{
112
        motor_l_set(FORWARD, 200);
113
        if(turnDistance!=21)motor_r_set(FORWARD, 230);
114
        else motor_r_set(FORWARD, 210);
115
        int colors[5];
116
        updateLine(colors);
117
        int position = lineLocate(colors);
118
        if(position>3 || position<-3)turnDistance++;
119

    
120
        if(turnDistance>20)
121
        {
122
        turnDistance=21;
123
        
124
                if(position<3 && position>-3)
125
                {
126
                        turnDistance = 0;
127
                        return 0;
128
                }        
129
        }
130
        return 1;
131
}
132

    
133

    
134
/**
135
 * Implements the right merge, assuming a line exists to the right.  Works by
136
 * turning off the line at an increasing angle and waiting to hit another line
137
 * on the right.
138
 */
139
int mergeRight()
140
{
141
        motor_r_set(FORWARD, 200);
142
        if(turnDistance!=21)motor_l_set(FORWARD, 230);
143
        else motor_l_set(FORWARD, 210);
144
        int colors[5];
145
        updateLine(colors);
146
        int position = lineLocate(colors);
147
        if(position>3 || position<-3)turnDistance++;
148

    
149
        if(turnDistance>20)
150
        {
151
        turnDistance=21;
152

    
153
                if(position<3 && position>-3)
154
                {
155
                        turnDistance = 0;
156
                        return 0;
157
                } 
158
        }
159
        return 1;
160
}
161

    
162

    
163

    
164
/**
165
 * Turns left at a cross of two lines.  Assumes that we are at lines in a cross
166
 * pattern, and turns until it sets straight on the new line.
167
 * @return 0 if turn finishes otherwise return 1
168
 */
169
int turnLeft()
170
{
171
        /*motor_l_set(BACKWARD, 200);
172
        motor_r_set(FORWARD, 200);
173
        int colors[5];
174
        updateLine(colors);
175
        int position = lineLocate(colors);
176
        if(position>2 || position<-2)turnDistance++;
177
        if(turnDistance>1)
178
        {
179
                if(position<3 && position>-3)
180
                {
181
                        turnDistance = 0;
182
                         return 0;
183
                }
184
        }
185
        return 1;*/
186

    
187
        motor_l_set(BACKWARD,200);
188
        motor_r_set(FORWARD,200);
189
        if(!encoderReset)
190
        {
191
            encoderStart = encoder_get_x(RIGHT);
192
            encoderReset = 1;
193
        }
194

    
195
        if(encoder_get_x(RIGHT) < encoderStart)
196
        {
197
            encoderStart = 0;
198
            // Temporary: display an "error message" in case of overflow.
199
            // Using this for debugging, take it out soon!
200
            motor_l_set(FORWARD,0);
201
            motor_r_set(FORWARD,0);
202
            orb_set_color(WHITE);
203
            delay_ms(2000);
204
        }
205

    
206
        if(encoder_get_x(RIGHT) - encoderStart > 300)
207
        {
208
            encoderReset = 0;
209
            return 0;
210
        }
211
        return 1;
212
}
213

    
214

    
215

    
216
/**
217
 * Turns right at a cross of two lines.  Assumes that we are at lines in a cross
218
 * pattern, and turns until it sets straight on the new line.
219
 * @return 0 if the turn finishes otherwise return 1
220
 */
221
int turnRight()
222
{
223
        motor_r_set(BACKWARD, 200);
224
        motor_l_set(FORWARD, 200);
225
        int colors[5];
226
        updateLine(colors);
227
        int position = lineLocate(colors);
228
        if(position>2 || position<-2)turnDistance++;
229
        if(turnDistance>1) 
230
        {
231
                if(position<3 && position>-3)
232
                {
233
                        turnDistance = 0;
234
                         return 0;
235
                }
236
        }
237
        return 1;
238
}
239

    
240

    
241

    
242
int getBarCode()
243
{
244
        if(barCodePosition != CODESIZE)
245
        return NOBARCODE ;
246
    else
247
    {
248
        int temp = 0;
249
        for(int i=0; i<CODESIZE; i++)
250
            temp += (barCode[i] << i);
251
        barCodePosition = 0;
252
        return temp;
253
    }
254
}
255

    
256

    
257

    
258
void updateLine(int* values)
259
{        
260
        for(int i = 0; i<5; i++)
261
                values[i] = (read_line(4-i) < LINE_COLOR ? LWHITE : LBLACK);
262
}
263

    
264

    
265

    
266
int lineLocate(int* colors)
267
{
268
        int i;
269
        int wsum = 0;
270
        int count=0;
271

    
272
        for(i = 0; i<5; i++)
273
        {
274
                count += colors[i]/2;
275
                wsum += (i)*colors[i];
276
        }
277
        if(count==0)
278
                return NOLINE;        
279
        if(count==5)
280
                return FULL_LINE;
281
        return (wsum/count)-4; // Subtract 4 to center the index around the center.
282
}
283

    
284

    
285
// new version by Alex
286
void updateBarCode()
287
{
288
    // USING THESE GLOBAL VARIABLES
289
    // global int duration = 0;
290
    // global int lastColor = 0;
291
    // global int barCodePosition = 0;
292
    // global char isReset = 0;
293

    
294
    // Average the readings of the last 2 sensors
295
    if(read_line(6) - read_line(7) > 150 || read_line(6) - read_line(7) < -150)
296
    {
297
        return;
298
    }
299

    
300
    int curReading = (read_line(6) + read_line(7)) / 2;
301
    int curColor;
302

    
303
    if(curReading > BLACK_THRESHOLD)
304
        curColor = LBLACK;
305
    else if(curReading < GREY_THRESHOLD)
306
        curColor = LWHITE;
307
    else
308
        curColor = LGREY;
309

    
310
    // Just an error check
311
    if(barCodePosition > CODESIZE)
312
    {
313
        barCodePosition = 0;
314
    }
315

    
316
    // We are only interested in consecutive color values
317
    if(curColor == lastColor)
318
    {
319
        duration++;
320
    }
321
    else
322
    {
323
        duration = 0;
324
        lastColor = curColor;
325
    }
326

    
327
    if(duration > MAX_DURATION)
328
    {
329
        // Now we assume our reading is significant - a bit, or a white space
330

    
331
        // Only read a value if we have read 0 first (isReset == 1)
332
        if(isReset && (curColor == LBLACK || curColor == LGREY) )
333
        {
334
            isReset = 0;
335
            usb_puts("Read barcode bit: ");
336
            usb_puti(barCodePosition);
337
            usb_puts(" = ");
338
            usb_puti(curColor);
339
            usb_puts(", curReading = ");
340
            usb_puti(curReading);
341
            usb_puts(".\n");
342

    
343
            barCode[barCodePosition++] = (curColor == LBLACK) ? 1 : 0;
344
        }
345
        else if(curColor == LWHITE)
346
        {
347
            isReset = 1;
348
        }
349
    }
350
    if(duration > TIMEOUT_DURATION && barCodePosition != 0)
351
    {
352
        usb_puts("TIMED OUT. BARCODE READER RESET.\n");
353
        barCodePosition = 0;
354
        duration = 0;
355
    }
356
}
357

    
358
// Dan's version
359
/*void updateBarCode()
360
{
361
        //! Note: currently only uses one of the barcode sensors.
362

363
        //maps the sensors to the analog input ports
364
        int ports[2] = {8,1};
365
        int current[2];
366
//        current[0] = analog_get10(ports[0]);
367
        //current[1] = analog_get10(ports[1]);
368
    current[1] = read_line(6);
369

370
        if(current[1] > 150)
371
        {
372
                if(countHi++ == 0) 
373
                {
374
                        avg = 500;
375
                        maxAvg = 500;
376
                }
377
                else
378
                {
379
                        avg = 3*avg + current[1];
380
                        avg/=4;
381
                        maxAvg = max(maxAvg, avg);
382
                }
383
        }
384
        else if(countHi > 5)
385
        {
386
                if(countLo++ > 15)
387
                {
388
                        countHi = countLo = 0;
389
                        if(maxAvg > 825)
390
                orb1_set_color(RED);
391
                        else orb1_set_color(BLUE);
392
                        barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
393
                }
394
        }
395
        else countHi/=2;
396
        if(countHi==0)countLo=0; 
397
}*/
398

    
399

    
400
int min(int x, int y){return x>y ? y : x;}
401
int max(int x, int y){return x<y ? y : x;}
402

    
403
void motorLeft(int speed){
404
        ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);
405
}
406

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