Statistics
| Revision:

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

History | View | Annotate | Download (5.71 KB)

1
/**
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
#include "lineFollow.h"
14

    
15
#define CODESIZE 5 
16

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

    
23
int turnDistance=0;
24
int intersectionFilter=0;
25
int disableBarCode=0;
26

    
27

    
28
/** 
29
 * Initializes line following. Must be called before other line-following
30
 * behavior will work.
31
 */
32
void lineFollow_init()
33
{
34
        analog_init(0);
35
        lost = 0;
36
        intersectionFilter=0;
37
        disableBarCode=0;
38
}
39

    
40

    
41
/** 
42
 * Follows a line at the given speed.
43
 * @param speed The speed with which to follow the line.
44
 */
45
int lineFollow(int speed)
46
{
47
        int colors[5];
48
        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
        else if(position == FULL_LINE)
65
        {
66
                if(intersectionFilter++>4)
67
                {
68
                        orb2_set_color(RED);
69
                        barCodePosition=0;
70
                        disableBarCode=50;
71
                }
72
        }
73
        //on line
74
        else
75
        {
76
                position*=30;
77
                orb2_set_color(ORB_OFF);
78
                motorLeft(min(speed+position, 255));
79
                motorRight(min(speed-position, 255));
80
                lost=0;
81
                intersectionFilter=0;
82
        }
83

    
84
        if(disableBarCode--)
85
        {
86
                if(disableBarCode)return NOBARCODE;
87
                return INTERSECTION;
88
        }
89
        updateBarCode();
90
        return getBarCode();
91
}
92

    
93

    
94
/**
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
int mergeLeft()
100
{
101
        motor_l_set(FORWARD, 200);
102
        if(turnDistance!=21)motor_r_set(FORWARD, 230);
103
        else motor_r_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
 * 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
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
/**
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
 * @return 0 if turn finishes otherwise return 1
157
 */
158
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
/**
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
 * @return 0 if the turn finishes otherwise return 1
183
 */
184
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
        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
}
202

    
203

    
204

    
205

    
206
int getBarCode()
207
{
208
        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

    
217

    
218

    
219
void updateLine(int* values)
220
{        
221
        int ports[5] = {13, 12, 3, 2, 9};
222
        for(int i = 0; i<5; i++)
223
                values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
224
}
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
                count += colors[i]/2;
237
                wsum += (i)*colors[i];
238
        }
239
        if(count==0)
240
                return NOLINE;        
241
        if(count==5)
242
                return FULL_LINE;
243
        return (wsum/count)-4;
244
}
245

    
246

    
247
void updateBarCode()
248
{
249

    
250
        //NOTE: currently only uses one of the barcode sensors.
251

    
252
        //maps the sensors to the analog input ports
253
        int ports[2] = {8,1};
254
        int current[2];
255
//        current[0] = analog_get10(ports[0]);
256
        current[1] = analog_get10(ports[1]);
257

    
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
        else if(countHi>5)
273
        {
274
                if(countLo++>15)
275
                {
276
                        countHi=countLo=0;
277
                        if(maxAvg>825)orb1_set_color(RED);
278
                        else orb1_set_color(BLUE);
279
                        barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
280
                }
281
        }
282
        else countHi/=2;
283
        if(countHi==0)countLo=0; 
284
}
285

    
286

    
287
//! A simple function to return the minimum of two integers.
288
int min(int x, int y){return x>y ? y : x;}
289
//! A simple function to return the maximum of two integers.
290
int max(int x, int y){return x<y ? y : x;}
291

    
292
void motorLeft(int speed){
293
        ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127);
294
}
295

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