Revision 1858

View differences:

trunk/code/projects/traffic_navigation/lineFollow.c
1
#include "lineFollow.h"
1 2

  
2
#include <lineFollow.h>
3

  
4
#define NOBARCODE -2
5 3
#define CODESIZE 5 
6 4

  
7 5
int countHi = 0;
......
10 8
int barCode[ CODESIZE ];
11 9
int barCodePosition=0;
12 10

  
11
int turnDistance=0;
12
int intersectionFilter=0;
13
int disableBarCode=0;
14

  
15

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

  
19 24

  
20 25

  
21
int lineFollow()
26
int lineFollow(int speed)
22 27
{
23
	int colors[6];
28
	int colors[5];
24 29
	int position;
25 30
	
26 31

  
......
37 42
			return LINELOST;
38 43
		}
39 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
	}
40 54
	//on line
41 55
	else
42 56
	{
43 57
		position*=30;
44 58
		orb2_set_color(ORB_OFF);
45
		motorLeft(min(SPEED+position, 150));
46
		motorRight(min(SPEED-position, 150));
59
		motorLeft(min(speed+position, 255));
60
		motorRight(min(speed-position, 255));
47 61
		lost=0;
62
		intersectionFilter=0;
48 63
	}
64

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

  
53
int getBarCode(){
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
{
54 169
	if(barCodePosition!=CODESIZE) return NOBARCODE ;
55 170
	int temp = 0;
56 171
	int i;
......
64 179

  
65 180
void updateLine(int* values)
66 181
{	
182
	int ports[5] = {13, 12, 3, 2, 9};
67 183
	for(int i = 0; i<5; i++)
68
		values[i] = assignColor(i, updateIR(i));
184
		values[i] = analog_get10(ports[i])<150 ? LWHITE : LBLACK;
69 185
}
70 186

  
71 187

  
72
int updateIR(int n)
73
{
74
	//maps the sensors to the analog input ports
75
	int ports[5] = {13, 12, 3, 2, 9};
76 188

  
77
	return analog_get10(ports[n]);
78
}
79

  
80

  
81

  
82
int assignColor(int port, int analog)
83
{
84
	if (analog < 150)
85
		return  0;
86
	else
87
		return  2;
88
}
89

  
90

  
91 189
int lineLocate(int* colors)
92 190
{
93 191
	int i;
......
100 198
		wsum += (i)*colors[i];
101 199
	}
102 200
	if(count==0)
103
			return NOLINE;	
104
		
201
		return NOLINE;	
202
	if(count==5)
203
		return FULL_LINE;
105 204
	return (wsum/count)-4;
106 205
}
107 206

  
108 207

  
109 208
void updateBarCode()
110 209
{
210

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

  
111 213
	//maps the sensors to the analog input ports
112 214
	int ports[2] = {8,1};
113 215
	int current[2];
114
//	current[0] = analog_get10(8);
115
	current[1] = analog_get10(1);
116
//	usb_puti(analog_get10(8));
117
//	usb_putc('\t');
118
//	usb_puti(analog_get10(1));
119
//	usb_putc('\n');
216
//	current[0] = analog_get10(ports[0]);
217
	current[1] = analog_get10(ports[1]);
120 218

  
121 219
	if(current[1]>500)
122 220
	{
......
139 237
			countHi=countLo=0;
140 238
			if(maxAvg>825)orb1_set_color(RED);
141 239
			else orb1_set_color(BLUE);
142
			barCode[barCodePosition++] = (maxAvg > 825 ? 2:1)-1;
240
			barCode[barCodePosition++] = maxAvg > 825 ? 1:0;
143 241
		}
144 242
	}
145 243
	else countHi/=2;
trunk/code/projects/traffic_navigation/lineFollow.h
3 3
 #ifndef _LINEFOLLOW_H_
4 4
 #define _LINEFOLLOW_H_
5 5

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

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

  
17

  
18

  
11 19
/* 	lineFollow_init
12 20
	Must call before lineFollow
13 21
	Turns analog loop off
14 22
*/ 
15
void lineFollow_init();
23
void lineFollow_init(void);
16 24

  
17 25
/*	lineFollow
18 26
	Must call lineFollow first
19 27
	Must be called inside a loop
20 28
*/
21
int lineFollow();
29
int lineFollow(int speed);
22 30

  
23
/*	assignColor
24
	Converts an analog color to WHITE or BLACK
31
/*	turnLeft turnRight mergeLeft mergeRight
32
	Must be called inside a loop
33
	returns 0 when complete
25 34
*/
26
int assignColor(int port, int analog);
35
int turnLeft(void);
36
int turnRight(void);
37
int mergeLeft(void);
38
int mergeRight(void);
27 39

  
28
/*	updateIR
29
	Gets the value of the nth line sensor
30
*/
31
int updateIR(int n);
32

  
33 40
/*	updateLine
41
	Reads in the analog values
34 42
	Fills the given array with WHITE
35 43
	or BLACK representing the line
36 44
*/
......
43 51
*/
44 52
int lineLocate(int* colors);
45 53

  
46
//Not implemented yet, returns ???
47
void updateBarCode();
54
/*	updatebarCode
55
	Reads in and processes
56
	bar code data
57
*/
58
void updateBarCode(void);
48 59

  
49
/*	min
50
	returns the minimum of two values
60
/*	getBarCode
61
	returns a bar code, if
62
	available, otherwise NOBARCODE
51 63
*/
64
int getBarCode(void);
65

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

  
54 72
/*	motorLeft
55 73
	Commands the left motor

Also available in: Unified diff