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