Project

General

Profile

Revision 1535

Added by John Sexton over 14 years ago

Added compiled binaries to bom_refactor branch. Also edited Push-Pull to test functionality of new BOM IR vector functions.

View differences:

branches/bom_refactor/code/behaviors/formation_control/push_pull/push_pull.c
3 3
#include <wl_basic.h>
4 4
#include <stdlib.h>
5 5

  
6
/* Struct for storing vector components */
7
typedef struct {
8
	int x;
9
	int y;
10
} Vector;
11

  
12

  
13
/* Function Prototypes */
14
static int get_bom_vector(Vector*);
15

  
16

  
17
/*******************************
18
 * BOM Vector Component Tables *
19
 *******************************/
20

  
21
/*
22
 * The x component of each BOM detector (indexed from 0 to 15)
23
 * was calculated using the following formula:
24
 *
25
 *		x_comp[i] = round(25 * cos ( 2 * pi / 16 * i) )
26
 *
27
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
28
 * this effectively calculates the x component of the emitter vector where
29
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
30
 */
31
static const signed int x_comp[16] = {
32
	25,
33
	23,
34
	17,
35
	9,
36
	0,
37
	-9,
38
	-17,
39
	-23,
40
	-25,
41
	-23,
42
	-17,
43
	-9,
44
	0,
45
	9,
46
	17,
47
	23
48
};
49

  
50

  
51
/*
52
 * The y component of each BOM detector (indexed from 0 to 15)
53
 * was calculated using the following formula:
54
 *
55
 *		y_comp[i] = round(25 * sin ( 2 * pi / 16 * i) )
56
 *
57
 * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
58
 * this effectively calculates the y component of the emitter vector where
59
 * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
60
 */
61
static signed int y_comp[16] = {
62
	0,
63
	9,
64
	17,
65
	23,
66
	25,
67
	23,
68
	17,
69
	9,
70
	0,
71
	-9,
72
	-17,
73
	-23,
74
	-25,
75
	-23,
76
	-17,
77
	-9
78
};
79

  
80

  
81

  
82 6
int main (void) {
83 7

  
84
	/* Store current BOM readings and use them as a weighting factor */
85
	uint8_t intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
8
	Vector v;
9
	int i;
10
	int bomVals[16];
86 11

  
87
	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
88
	 * components. Calculated by multiplying the intensity by the x and y
89
	 * component respectively (x and y components are stored in the tables
90
	 * above). */
91
	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
92
	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
93
	
94
	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
95
	 * components for the entire robot. */
96
	long net_x_comp = 0;
97
	long net_y_comp = 0;
98

  
99
	/* Variables used to normalize the net component values */
100
	int total_intensity = 0;
101
	int normalized_net_x_comp = 0;
102
	int normalized_net_y_comp = 0;
103

  
104
	int i = 0;
105

  
106 12
	dragonfly_init(ALL_ON);
107 13
	xbee_init();
108 14
	encoders_init();
......
115 21

  
116 22
	while (1) {
117 23

  
118
		/* Make sure to clear our accumulators */
119
		net_x_comp = 0;
120
		net_y_comp = 0;
121
		total_intensity = 0;
122

  
123
		bom_print_usb(NULL);
124

  
125 24
		bom_refresh(BOM_ALL);
126 25
		for (i = 0; i < 16; i++) {
127
			/* BOM intensity is actually measured as more intense = closer to 0 */
128
			intensity[i] = 255 - bom_get(i);
129
			weighted_x_comp[i] = intensity[i] * x_comp[i];
130
			weighted_y_comp[i] = intensity[i] * y_comp[i];
131
			net_x_comp += weighted_x_comp[i];
132
			net_y_comp += weighted_y_comp[i];
133
			total_intensity += intensity[i];
26
			bomVals[i] = bom_get(i);
134 27
		}
135 28

  
136
		if (total_intensity > 0) {
137
			normalized_net_x_comp = net_x_comp / total_intensity;
138
			normalized_net_y_comp = net_y_comp / total_intensity;
139
		}
29
		bom_print_usb(bomVals);
140 30

  
31
		bom_get_norm_vector(&v, bomVals);
32

  
141 33
		usb_puts("x: ");	
142
		usb_puti(normalized_net_x_comp);
34
		usb_puti(v.x);
143 35
		usb_puts("\ty: ");
144
		usb_puti(normalized_net_y_comp);
36
		usb_puti(v.y);
145 37
		usb_puts("\n");
146 38

  
147 39
		delay_ms(50);
......
151 43
	while(1);
152 44

  
153 45
}
154

  
155

  
156
static int get_bom_vector(Vector* bom_vector) {
157

  
158
	return 0;
159

  
160
}
branches/bom_refactor/code/lib/include/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