Revision 1534
Added two BOM IR vector functions to calculate resultant IR vector and normalized resultant IR vector.
branches/bom_refactor/code/projects/libdragonfly/bom.c | ||
---|---|---|
42 | 42 |
//so you need to iterate through the mux index in the following order if you want to get |
43 | 43 |
//the detector readings in order: |
44 | 44 |
static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa}; |
45 |
|
|
46 |
|
|
47 |
/* ***************************** |
|
48 |
* BOM Vector Component Tables * |
|
49 |
**************************** **/ |
|
50 |
|
|
51 |
/* |
|
52 |
* The x component of each BOM detector (indexed from 0 to 15) |
|
53 |
* was calculated using the following formula: |
|
54 |
* |
|
55 |
* x_comp[i] = fix(25 * cos ( 2 * pi / 16 * i) ) |
|
56 |
* |
|
57 |
* where "fix" rounds towards 0. If the BOM detectors were superimposed |
|
58 |
* onto a 2 dimensional Cartesian space, this effectively calculates the |
|
59 |
* x component of the emitter vector where emitter 0 corresponds to an |
|
60 |
* angle of 0 radians, 4 -> pi/2, 8 -> pi, ect. |
|
61 |
*/ |
|
62 |
static const signed int x_comp[16] = { |
|
63 |
25, |
|
64 |
23, |
|
65 |
17, |
|
66 |
9, |
|
67 |
0, |
|
68 |
-9, |
|
69 |
-17, |
|
70 |
-23, |
|
71 |
-25, |
|
72 |
-23, |
|
73 |
-17, |
|
74 |
-9, |
|
75 |
0, |
|
76 |
9, |
|
77 |
17, |
|
78 |
23 |
|
79 |
}; |
|
80 |
|
|
81 |
|
|
82 |
/* |
|
83 |
* The y component of each BOM detector (indexed from 0 to 15) |
|
84 |
* was calculated using the following formula: |
|
85 |
* |
|
86 |
* y_comp[i] = fix(25 * sin ( 2 * pi / 16 * i) ) |
|
87 |
* |
|
88 |
* where "fix" rounds towards 0. If the BOM detectors were superimposed |
|
89 |
* onto a 2 dimensional Cartesian space, this effectively calculates the |
|
90 |
* x component of the emitter vector where emitter 0 corresponds to an |
|
91 |
* angle of 0 radians, 4 -> pi/2, 8 -> pi, ect. |
|
92 |
*/ |
|
93 |
static signed int y_comp[16] = { |
|
94 |
0, |
|
95 |
9, |
|
96 |
17, |
|
97 |
23, |
|
98 |
25, |
|
99 |
23, |
|
100 |
17, |
|
101 |
9, |
|
102 |
0, |
|
103 |
-9, |
|
104 |
-17, |
|
105 |
-23, |
|
106 |
-25, |
|
107 |
-23, |
|
108 |
-17, |
|
109 |
-9 |
|
110 |
}; |
|
45 | 111 |
|
46 | 112 |
// internal function prototypes |
47 | 113 |
static void bom_select(char which); |
... | ... | |
228 | 294 |
} |
229 | 295 |
|
230 | 296 |
/** |
297 |
* Compute the net resultant BOM IR vector by scaling each IR unit vector by its intensity |
|
298 |
* and summing over all IR LEDs. |
|
299 |
* |
|
300 |
* @param v Pointer to Vector struct to be filled by this function with an x and y net vector |
|
301 |
* component. |
|
302 |
* |
|
303 |
* @param usrBOMvals Pointer to array which holds 16 raw BOM readings which can be used if user |
|
304 |
* has already collected BOM information instead of collecting a new data set from the BOM. |
|
305 |
* |
|
306 |
* @return Exit status - Zero for success; negative on error. |
|
307 |
**/ |
|
308 |
int bom_get_vector(Vector* v, int* usrBOMvals) { |
|
309 |
|
|
310 |
/* Store current BOM readings and use them as a weighting factor */ |
|
311 |
int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
312 |
|
|
313 |
/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness") |
|
314 |
* components. Calculated by multiplying the intensity by the x and y |
|
315 |
* component respectively (x and y components are stored in the tables |
|
316 |
* above). */ |
|
317 |
int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
318 |
int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
319 |
|
|
320 |
/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness") |
|
321 |
* components for the entire robot. */ |
|
322 |
long net_x_comp = 0; |
|
323 |
long net_y_comp = 0; |
|
324 |
|
|
325 |
int i = 0; |
|
326 |
|
|
327 |
/* BOM intensity is actually measured as more intense = closer to 0 */ |
|
328 |
if (usrBOMvals) { |
|
329 |
/* Use BOM values collected by user */ |
|
330 |
for (i = 0; i < 16; i++) { |
|
331 |
intensity[i] = 255 - usrBOMvals[i]; |
|
332 |
} |
|
333 |
} else { |
|
334 |
/* Collect new set of BOM data */ |
|
335 |
bom_refresh(BOM_ALL); |
|
336 |
for (i = 0; i < 16; i++) { |
|
337 |
intensity[i] = 255 - bom_get(i); |
|
338 |
} |
|
339 |
} |
|
340 |
|
|
341 |
/* Calculate weighted vector components and accumulate vector sum */ |
|
342 |
for (i = 0; i < 16; i++) { |
|
343 |
weighted_x_comp[i] = intensity[i] * x_comp[i]; |
|
344 |
weighted_y_comp[i] = intensity[i] * y_comp[i]; |
|
345 |
net_x_comp += weighted_x_comp[i]; |
|
346 |
net_y_comp += weighted_y_comp[i]; |
|
347 |
} |
|
348 |
|
|
349 |
/* Fill the Vector struct */ |
|
350 |
v->x = net_x_comp; |
|
351 |
v->y = net_y_comp; |
|
352 |
|
|
353 |
return 0; |
|
354 |
|
|
355 |
} |
|
356 |
|
|
357 |
/** |
|
358 |
* Compute the normalized net resultant BOM IR vector by scaling each IR unit vector by its |
|
359 |
* intensity and summing over all IR LEDs. |
|
360 |
* |
|
361 |
* @param v Pointer to Vector struct to be filled by this function with an x and y net vector |
|
362 |
* component. |
|
363 |
* |
|
364 |
* @param usrBOMvals Pointer to array which holds 16 raw BOM readings which can be used if user |
|
365 |
* has already collected BOM information instead of collecting a new data set from the BOM. |
|
366 |
* |
|
367 |
* @return Exit status - Zero for success; negative on error. |
|
368 |
**/ |
|
369 |
int bom_get_norm_vector(Vector* v, int* usrBOMvals) { |
|
370 |
|
|
371 |
/* Store current BOM readings and use them as a weighting factor */ |
|
372 |
int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
373 |
|
|
374 |
/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness") |
|
375 |
* components. Calculated by multiplying the intensity by the x and y |
|
376 |
* component respectively (x and y components are stored in the tables |
|
377 |
* above). */ |
|
378 |
int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
379 |
int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
|
380 |
|
|
381 |
/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness") |
|
382 |
* components for the entire robot. */ |
|
383 |
long net_x_comp = 0; |
|
384 |
long net_y_comp = 0; |
|
385 |
|
|
386 |
/* Variables used to normalize the net component values */ |
|
387 |
int total_intensity = 0; |
|
388 |
int normalized_net_x_comp = 0; |
|
389 |
int normalized_net_y_comp = 0; |
|
390 |
|
|
391 |
int i = 0; |
|
392 |
|
|
393 |
/* BOM intensity is actually measured as more intense = closer to 0 */ |
|
394 |
if (usrBOMvals) { |
|
395 |
/* Use BOM values collected by user */ |
|
396 |
for (i = 0; i < 16; i++) { |
|
397 |
intensity[i] = 255 - usrBOMvals[i]; |
|
398 |
} |
|
399 |
} else { |
|
400 |
/* Collect new set of BOM data */ |
|
401 |
bom_refresh(BOM_ALL); |
|
402 |
for (i = 0; i < 16; i++) { |
|
403 |
intensity[i] = 255 - bom_get(i); |
|
404 |
} |
|
405 |
} |
|
406 |
|
|
407 |
/* Calculate weighted vector components and accumulate vector sum */ |
|
408 |
for (i = 0; i < 16; i++) { |
|
409 |
weighted_x_comp[i] = intensity[i] * x_comp[i]; |
|
410 |
weighted_y_comp[i] = intensity[i] * y_comp[i]; |
|
411 |
net_x_comp += weighted_x_comp[i]; |
|
412 |
net_y_comp += weighted_y_comp[i]; |
|
413 |
total_intensity += intensity[i]; |
|
414 |
} |
|
415 |
|
|
416 |
/* Normalize the resultant vector components by the total intensity */ |
|
417 |
if (total_intensity > 0) { |
|
418 |
normalized_net_x_comp = net_x_comp / total_intensity; |
|
419 |
normalized_net_y_comp = net_y_comp / total_intensity; |
|
420 |
} |
|
421 |
|
|
422 |
/* Fill the Vector struct */ |
|
423 |
v->x = normalized_net_x_comp; |
|
424 |
v->y = normalized_net_y_comp; |
|
425 |
|
|
426 |
return 0; |
|
427 |
|
|
428 |
} |
|
429 |
|
|
430 |
/** |
|
231 | 431 |
* Print a histogram which shows the current BOM intensity values for each of the 16 BOM IR |
232 | 432 |
* sensors. The function will attempt to send the histogram data over USB. |
233 | 433 |
* |
234 | 434 |
* @param curBOMvals Pointer to an array of the current BOM values (the array must have |
235 | 435 |
* length 16). Use this to print values you have already collected. Otherwise pass in NULL |
236 | 436 |
* and bom_refresh() will be called and the current BOM intensity values will be collected. |
437 |
* @return Exit status - Zero for success; negative on error. |
|
237 | 438 |
**/ |
238 |
void bom_print_usb(int* usrBOMvals) {
|
|
439 |
int bom_print_usb(int* usrBOMvals) {
|
|
239 | 440 |
|
240 | 441 |
int i, j, max = -1; |
241 | 442 |
int curVals[16]; |
... | ... | |
279 | 480 |
usb_puts("\r\n"); |
280 | 481 |
} |
281 | 482 |
usb_puts("\r\n"); |
483 |
|
|
484 |
return 0; |
|
282 | 485 |
|
283 | 486 |
} |
284 | 487 |
|
branches/bom_refactor/code/projects/libdragonfly/bom.h | ||
---|---|---|
56 | 56 |
#define RBOM 2 |
57 | 57 |
|
58 | 58 |
|
59 |
/** @brief Struct for storing vector data. Used by bom_get_vector() functions **/ |
|
60 |
typedef struct vector { |
|
61 |
int x; |
|
62 |
int y; |
|
63 |
} Vector; |
|
64 |
|
|
65 |
|
|
59 | 66 |
/** @brief Initialize the bom according to bom type **/ |
60 | 67 |
void bom_init(char type); |
61 | 68 |
|
... | ... | |
68 | 75 |
/** @brief Compares all the values in bom_val[] and returns the index to the highest value element. **/ |
69 | 76 |
int bom_get_max(void); |
70 | 77 |
|
78 |
/** @brief Computes the net resultant BOM IR vector. **/ |
|
79 |
int bom_get_vector(Vector*, int*); |
|
80 |
|
|
81 |
/** @brief Computes the normalized net resultant BOM IR vector. **/ |
|
82 |
int bom_get_norm_vector(Vector*, int*); |
|
83 |
|
|
71 | 84 |
/** @brief Print snapshot of BOM intensity histogram over USB connection **/ |
72 |
void bom_print_usb(int*);
|
|
85 |
int bom_print_usb(int*);
|
|
73 | 86 |
|
74 | 87 |
/** @brief Computes the weighted average of all the bom readings to estimate the position and distance of another robot. **/ |
75 | 88 |
int bom_get_max10(int *dist); |
Also available in: Unified diff