Revision 1858
Updated line following code from line following project
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