root / trunk / code / projects / linefollowing / lineFollow.c @ 1851
History | View | Annotate | Download (3.18 KB)
1 | 1841 | djacobs | #include "lineFollow.h" |
---|---|---|---|
2 | |||
3 | 1842 | djacobs | #define NOBARCODE -2 |
4 | 1845 | dgurjar | #define CODESIZE 5 |
5 | 1851 | djacobs | #define INTERSECTION -25 |
6 | #define FULL_LINE -26 |
||
7 | 1841 | djacobs | |
8 | 1842 | djacobs | int countHi = 0; |
9 | int countLo = 0; |
||
10 | int maxAvg, avg;
|
||
11 | int barCode[ CODESIZE ];
|
||
12 | int barCodePosition=0; |
||
13 | 1841 | djacobs | |
14 | 1851 | djacobs | int turnDistance=0; |
15 | int intersection=0; |
||
16 | int disableBarCode=0; |
||
17 | |||
18 | |||
19 | 1841 | djacobs | void lineFollow_init()
|
20 | { |
||
21 | analog_init(0);
|
||
22 | lost = 0;
|
||
23 | 1851 | djacobs | intersection=0;
|
24 | disableBarCode=0;
|
||
25 | 1841 | djacobs | } |
26 | |||
27 | |||
28 | |||
29 | 1851 | djacobs | int lineFollow(int speed) |
30 | 1841 | djacobs | { |
31 | 1851 | djacobs | int colors[5]; |
32 | 1841 | djacobs | int position;
|
33 | |||
34 | |||
35 | updateLine(colors); |
||
36 | position = lineLocate(colors); |
||
37 | |||
38 | //not on line
|
||
39 | if(position == NOLINE)
|
||
40 | { |
||
41 | if(lost++>20) |
||
42 | { |
||
43 | orb2_set_color(GREEN); |
||
44 | motors_off(); |
||
45 | return LINELOST;
|
||
46 | } |
||
47 | } |
||
48 | 1851 | djacobs | else if(position == FULL_LINE) |
49 | { |
||
50 | if(intersection++>4) |
||
51 | { |
||
52 | orb2_set_color(RED); |
||
53 | barCodePosition=0;
|
||
54 | disableBarCode=15;
|
||
55 | return INTERSECTION;
|
||
56 | } |
||
57 | } |
||
58 | 1841 | djacobs | //on line
|
59 | else
|
||
60 | { |
||
61 | position*=30;
|
||
62 | orb2_set_color(ORB_OFF); |
||
63 | 1851 | djacobs | motorLeft(min(speed+position, 255));
|
64 | motorRight(min(speed-position, 255));
|
||
65 | 1841 | djacobs | lost=0;
|
66 | 1851 | djacobs | intersection=0;
|
67 | 1841 | djacobs | } |
68 | 1851 | djacobs | |
69 | if(disableBarCode)
|
||
70 | { |
||
71 | disableBarCode--; |
||
72 | return NOBARCODE;
|
||
73 | } |
||
74 | 1842 | djacobs | updateBarCode(); |
75 | return getBarCode();
|
||
76 | 1841 | djacobs | } |
77 | |||
78 | 1851 | djacobs | |
79 | int turnLeft()
|
||
80 | { |
||
81 | motor_l_set(BACKWARD, 200);
|
||
82 | motor_r_set(FORWARD, 200);
|
||
83 | int colors[5]; |
||
84 | updateLine(colors); |
||
85 | int position = lineLocate(colors);
|
||
86 | if(position==NOLINE)turnDistance++;
|
||
87 | else if(turnDistance>10) return 0;//turnDistance = 0; |
||
88 | return 1; |
||
89 | } |
||
90 | |||
91 | int turnRight()
|
||
92 | { |
||
93 | motor_r_set(BACKWARD, 200);
|
||
94 | motor_l_set(FORWARD, 200);
|
||
95 | int colors[5]; |
||
96 | updateLine(colors); |
||
97 | int position = lineLocate(colors);
|
||
98 | if(position==NOLINE)turnDistance++;
|
||
99 | else if(turnDistance>10) return 0;//turnDistance = 0; |
||
100 | return 1; |
||
101 | } |
||
102 | |||
103 | |||
104 | |||
105 | |||
106 | int getBarCode()
|
||
107 | { |
||
108 | 1842 | djacobs | if(barCodePosition!=CODESIZE) return NOBARCODE ; |
109 | int temp = 0; |
||
110 | int i;
|
||
111 | for(i=0; i<CODESIZE; i++) |
||
112 | temp += (barCode[i] << i); |
||
113 | barCodePosition = 0;
|
||
114 | return temp;
|
||
115 | } |
||
116 | 1841 | djacobs | |
117 | |||
118 | |||
119 | void updateLine(int* values) |
||
120 | { |
||
121 | 1851 | djacobs | int ports[5] = {13, 12, 3, 2, 9}; |
122 | 1841 | djacobs | for(int i = 0; i<5; i++) |
123 | 1851 | djacobs | values[i] = analog_get10(ports[i])<150 ? WHITE : BLACK;
|
124 | 1841 | djacobs | } |
125 | |||
126 | |||
127 | |||
128 | int lineLocate(int* colors) |
||
129 | { |
||
130 | int i;
|
||
131 | int wsum = 0; |
||
132 | int count=0; |
||
133 | |||
134 | for(i = 0; i<5; i++) |
||
135 | { |
||
136 | 1845 | dgurjar | count += colors[i]/2;
|
137 | 1841 | djacobs | wsum += (i)*colors[i]; |
138 | } |
||
139 | if(count==0) |
||
140 | 1851 | djacobs | return NOLINE;
|
141 | if(count==5) |
||
142 | return FULL_LINE;
|
||
143 | 1841 | djacobs | return (wsum/count)-4; |
144 | } |
||
145 | |||
146 | |||
147 | 1842 | djacobs | void updateBarCode()
|
148 | 1841 | djacobs | { |
149 | //maps the sensors to the analog input ports
|
||
150 | int ports[2] = {8,1}; |
||
151 | 1842 | djacobs | int current[2]; |
152 | // current[0] = analog_get10(8);
|
||
153 | current[1] = analog_get10(1); |
||
154 | // usb_puti(analog_get10(8));
|
||
155 | // usb_putc('\t');
|
||
156 | 1845 | dgurjar | // usb_puti(analog_get10(1));
|
157 | // usb_putc('\n');
|
||
158 | 1842 | djacobs | |
159 | if(current[1]>500) |
||
160 | { |
||
161 | if(countHi++==0) |
||
162 | { |
||
163 | avg = 500;
|
||
164 | maxAvg = 500;
|
||
165 | } |
||
166 | else
|
||
167 | { |
||
168 | avg = 3*avg + current[1]; |
||
169 | avg/=4;
|
||
170 | maxAvg = max(maxAvg, avg); |
||
171 | } |
||
172 | } |
||
173 | 1845 | dgurjar | else if(countHi>5) |
174 | 1842 | djacobs | { |
175 | if(countLo++>15) |
||
176 | { |
||
177 | countHi=countLo=0;
|
||
178 | 1845 | dgurjar | if(maxAvg>825)orb1_set_color(RED); |
179 | else orb1_set_color(BLUE);
|
||
180 | 1851 | djacobs | barCode[barCodePosition++] = maxAvg > 825 ? 1:0; |
181 | 1842 | djacobs | } |
182 | } |
||
183 | else countHi/=2; |
||
184 | if(countHi==0)countLo=0; |
||
185 | 1841 | djacobs | } |
186 | |||
187 | |||
188 | int min(int x, int y){return x>y ? y : x;} |
||
189 | 1842 | djacobs | int max(int x, int y){return x<y ? y : x;} |
190 | 1841 | djacobs | |
191 | void motorLeft(int speed){ |
||
192 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
193 | 1841 | djacobs | } |
194 | |||
195 | void motorRight(int speed){ |
||
196 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
197 | 1841 | djacobs | } |