root / trunk / code / projects / linefollowing / lineFollow.c @ 1845
History | View | Annotate | Download (2.4 KB)
1 | 1841 | djacobs | #include "lineFollow.h" |
---|---|---|---|
2 | |||
3 | 1842 | djacobs | #define NOBARCODE -2 |
4 | 1845 | dgurjar | #define CODESIZE 5 |
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 | 1845 | dgurjar | motorLeft(min(SPEED+position, 150));
|
45 | motorRight(min(SPEED-position, 150));
|
||
46 | 1841 | djacobs | 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 | 1845 | dgurjar | count += colors[i]/2;
|
99 | 1841 | djacobs | 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 | 1845 | dgurjar | // usb_puti(analog_get10(1));
|
118 | // usb_putc('\n');
|
||
119 | 1842 | djacobs | |
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 | 1845 | dgurjar | else if(countHi>5) |
135 | 1842 | djacobs | { |
136 | if(countLo++>15) |
||
137 | { |
||
138 | countHi=countLo=0;
|
||
139 | 1845 | dgurjar | if(maxAvg>825)orb1_set_color(RED); |
140 | else orb1_set_color(BLUE);
|
||
141 | 1842 | djacobs | 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 | 1845 | dgurjar | ((speed-=127)>=0)?motor_l_set(FORWARD, 160+speed*95/128):motor_l_set(BACKWARD, 160-speed*95/127); |
154 | 1841 | djacobs | } |
155 | |||
156 | void motorRight(int speed){ |
||
157 | 1845 | dgurjar | ((speed-=127)>=0)?motor_r_set(FORWARD, 160+speed*95/128):motor_r_set(BACKWARD, 160-speed*95/127); |
158 | 1841 | djacobs | } |