Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / lineFollow.c @ 1858

History | View | Annotate | Download (4.28 KB)

1
#include "lineFollow.h"
2

    
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
int turnDistance=0;
12
int intersectionFilter=0;
13
int disableBarCode=0;
14

    
15

    
16
void lineFollow_init()
17
{
18
        analog_init(0);
19
        lost = 0;
20
        intersectionFilter=0;
21
        disableBarCode=0;
22
}
23

    
24

    
25

    
26
int lineFollow(int speed)
27
{
28
        int colors[5];
29
        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
        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
        //on line
55
        else
56
        {
57
                position*=30;
58
                orb2_set_color(ORB_OFF);
59
                motorLeft(min(speed+position, 255));
60
                motorRight(min(speed-position, 255));
61
                lost=0;
62
                intersectionFilter=0;
63
        }
64

    
65
        if(disableBarCode--)
66
        {
67
                if(disableBarCode)return NOBARCODE;
68
                return INTERSECTION;
69
        }
70
        updateBarCode();
71
        return getBarCode();
72
}
73

    
74

    
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
        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
        int ports[5] = {13, 12, 3, 2, 9};
183
        for(int i = 0; i<5; i++)
184
                values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
185
}
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
                return NOLINE;        
202
        if(count==5)
203
                return FULL_LINE;
204
        return (wsum/count)-4;
205
}
206

    
207

    
208
void updateBarCode()
209
{
210

    
211
        //NOTE: currently only uses one of the barcode sensors.
212

    
213
        //maps the sensors to the analog input ports
214
        int ports[2] = {8,1};
215
        int current[2];
216
//        current[0] = analog_get10(ports[0]);
217
        current[1] = analog_get10(ports[1]);
218

    
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
                        barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
241
                }
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
}
258