Revision 775
implemented averaging with older data to smooth out object detection values
driver.c | ||
---|---|---|
59 | 59 |
#ifdef COLOR_DETECT |
60 | 60 |
/* set up color detect */ |
61 | 61 |
int rgb[3] = {0,0,0}; |
62 |
int rgb_old1[3] = {-1,0,0}; |
|
63 |
int rgb_old2[3] = {-1,0,0}; |
|
64 |
int rgb_avg[3] = {0,0,0}; |
|
62 | 65 |
|
63 | 66 |
//long int distance_red; |
64 | 67 |
//long int distance_green; |
... | ... | |
112 | 115 |
|
113 | 116 |
digital_output(GREEN_LED,0); |
114 | 117 |
|
115 |
ratio_gb = rgb[1] - rgb[2]; |
|
118 |
// check with older values |
|
119 |
if (rgb_old1[0] != -1 && rgb_old2[0] != -1) { |
|
120 |
rgb_avg[0] = (rgb[0]+rgb_old1[0]+rgb_old2[0])/3; |
|
121 |
rgb_avg[1] = (rgb[1]+rgb_old1[1]+rgb_old2[1])/3; |
|
122 |
rgb_avg[2] = (rgb[2]+rgb_old1[2]+rgb_old2[2])/3; |
|
123 |
} else { |
|
124 |
rgb_avg[0] = rgb[0]; |
|
125 |
rgb_avg[1] = rgb[1]; |
|
126 |
rgb_avg[2] = rgb[2]; |
|
127 |
} |
|
128 |
|
|
129 |
if (rgb_old1[0] == -1) { |
|
130 |
rgb_old1[0] = rgb[0]; |
|
131 |
rgb_old1[1] = rgb[1]; |
|
132 |
rgb_old1[2] = rgb[2]; |
|
133 |
} else { |
|
134 |
if (rgb_old1[0] < rgb[0]+20 |
|
135 |
&& rgb_old1[0] > rgb[0]-20) { |
|
136 |
rgb_old1[0] = rgb[0]; |
|
137 |
rgb_old2[0] = rgb_old1[0]; |
|
138 |
} |
|
139 |
if (rgb_old1[1] < rgb[1]+20 |
|
140 |
&& rgb_old1[1] > rgb[1]-20) { |
|
141 |
rgb_old1[1] = rgb[1]; |
|
142 |
rgb_old2[1] = rgb_old1[1]; |
|
143 |
} |
|
144 |
if (rgb_old1[2] < rgb[2]+20 |
|
145 |
&& rgb_old1[2] > rgb[2]-20) { |
|
146 |
rgb_old1[2] = rgb[2]; |
|
147 |
rgb_old2[2] = rgb_old1[2]; |
|
148 |
} |
|
149 |
} |
|
150 |
|
|
151 |
// check ratio |
|
152 |
ratio_gb = rgb_avg[1] - rgb_avg[2]; |
|
116 | 153 |
if (ratio_gb < -20) |
117 | 154 |
ratio_gb = 2; |
118 | 155 |
else if (ratio_gb > 20) |
... | ... | |
120 | 157 |
else |
121 | 158 |
ratio_gb = 1; |
122 | 159 |
|
123 |
if (rgb[0] > 200 && rgb[1] > 200 && rgb[2] > 200) {
|
|
160 |
if (rgb_avg[0] > 200 && rgb_avg[1] > 200 && rgb_avg[2] > 200) {
|
|
124 | 161 |
// we think this is open area |
125 | 162 |
move(170,0); |
126 | 163 |
} else if (ratio_gb == COLOR_WALL) { |
... | ... | |
144 | 181 |
distance_green = distance_green * rgb[0]/255;*/ |
145 | 182 |
|
146 | 183 |
usb_puts("R:"); |
147 |
usb_puti(rgb[0]); |
|
184 |
usb_puti(rgb_avg[0]);
|
|
148 | 185 |
usb_puts("\tG:"); |
149 |
usb_puti(rgb[1]); |
|
186 |
usb_puti(rgb_avg[1]);
|
|
150 | 187 |
usb_puts("\tB:"); |
151 |
usb_puti(rgb[2]); |
|
188 |
usb_puti(rgb_avg[2]);
|
|
152 | 189 |
usb_puts("\t\tratio_gb:"); |
153 | 190 |
usb_puti(ratio_gb); |
154 | 191 |
usb_puts("\r\n"); |
... | ... | |
177 | 214 |
} |
178 | 215 |
|
179 | 216 |
// object manip goes here |
217 |
orb_set_color(YELLOW); |
|
180 | 218 |
while(1) { |
181 | 219 |
wl_do(); |
182 | 220 |
} |
Also available in: Unified diff