Project

General

Profile

Revision 1534

Added by John Sexton over 10 years ago

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

View differences:

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