Revision 1858 trunk/code/projects/traffic_navigation/lineFollow.c

View differences:

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;

Also available in: Unified diff