## Revision 1860

View differences:

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
```
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

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