Revision 1860

View differences:

trunk/code/projects/traffic_navigation/lineFollow.h
1
#include <dragonfly_lib.h>
2

  
3
 #ifndef _LINEFOLLOW_H_
4
 #define _LINEFOLLOW_H_
5

  
6
#define LWHITE			0
7
#define LGREY			1
8
#define LBLACK	 		2
9
#define CENTER			3
10
#define NOLINE			-42
11
#define LINELOST		-1
12

  
13
#define NOBARCODE 		-2
14
#define INTERSECTION 		-25
15
#define FULL_LINE 		-26
16

  
17

  
18

  
19
/* 	lineFollow_init
20
	Must call before lineFollow
21
	Turns analog loop off
22
*/ 
23
void lineFollow_init(void);
24

  
25
/*	lineFollow
26
	Must call lineFollow first
27
	Must be called inside a loop
28
*/
29
int lineFollow(int speed);
30

  
31
/*	turnLeft turnRight mergeLeft mergeRight
32
	Must be called inside a loop
33
	returns 0 when complete
34
*/
35
int turnLeft(void);
36
int turnRight(void);
37
int mergeLeft(void);
38
int mergeRight(void);
39

  
40
/*	updateLine
41
	Reads in the analog values
42
	Fills the given array with WHITE
43
	or BLACK representing the line
44
*/
45
void updateLine(int* values); 
46

  
47
/*	lineLocate
48
	Finds the location of the line
49
	Outputs positive for right side
50
	Negative for left, or NOLINE if a line is not found
51
*/
52
int lineLocate(int* colors);
53

  
54
/*	updatebarCode
55
	Reads in and processes
56
	bar code data
57
*/
58
void updateBarCode(void);
59

  
60
/*	getBarCode
61
	returns a bar code, if
62
	available, otherwise NOBARCODE
63
*/
64
int getBarCode(void);
65

  
66
/*	min max
67
	returns the minimum/maximum of two values
68
*/
69
int min(int x, int y);
70
int max(int x, int y);
71

  
72
/*	motorLeft
73
	Commands the left motor
74
	Cannot be used to stop
75
	0-126 are backward
76
	127-255 are forward
77
*/
78
void motorLeft(int speed);
79

  
80
/*	motorRight
81
	Commands the right motor
82
	Cannot be used to stop
83
	0-126 are backward
84
	127-255 are forward
85
*/
86
void motorRight(int speed);
87

  
88
/*	lost
89
	Internal counter to detect if the line was lost
90
*/
91
int lost;
92

  
93
#endif
trunk/code/projects/traffic_navigation/lineFollow.c
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

  
trunk/code/projects/traffic_navigation/main.c
7 7

  
8 8
#include <dragonfly_lib.h>
9 9
#include <wl_basic.h>
10
#include "lineFollow.h"
10
#include "../linefollowing/lineFollow.h"
11 11

  
12 12

  
13 13
/*States*/

Also available in: Unified diff