## Revision 1534

Added two BOM IR vector functions to calculate resultant IR vector and normalized resultant IR vector.

View differences:

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

Also available in: Unified diff