Project

General

Profile

Revision 1511

Renamed Push-Pull behavior code file to "push_pull.c"

View differences:

trunk/code/behaviors/formation_control/push_pull/main.c
1
#include <stdint.h>
2
#include <dragonfly_lib.h>
3
#include <wl_basic.h>
4
#include <stdlib.h>
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(100 * 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
	100,
33
	92,
34
	71,
35
	38,
36
	0,
37
	-38,
38
	-71,
39
	-92,
40
	-100,
41
	-92,
42
	-71,
43
	-38,
44
	0,
45
	38,
46
	71,
47
	92
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(100 * 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
	38,
64
	71,
65
	92,
66
	100,
67
	92,
68
	71,
69
	38,
70
	0,
71
	-38,
72
	-71,
73
	-92,
74
	-100,
75
	-92,
76
	-71,
77
	-38
78
};
79

  
80

  
81

  
82
int main (void) {
83

  
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};
86

  
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
	dragonfly_init(ALL_ON);
107
	xbee_init();
108
	encoders_init();
109

  
110
	orbs_set_color(BLUE, GREEN);
111
	delay_ms(1000);
112
	orbs_set_color(GREEN, BLUE);
113
	delay_ms(1000);
114
	orbs_set_color(RED, RED);
115

  
116
	while (1) {
117

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

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

  
134
		if (total_intensity > 0) {
135
			normalized_net_x_comp = net_x_comp / total_intensity;
136
			normalized_net_y_comp = net_y_comp / total_intensity;
137
		}
138

  
139
		usb_puts("x: ");	
140
		usb_puti(normalized_net_x_comp);
141
		usb_puts("\ty: ");
142
		usb_puti(normalized_net_y_comp);
143
		usb_puts("\n");
144

  
145
		delay_ms(50);
146

  
147
	}
148

  
149
	while(1);
150

  
151
}
152

  
153

  
154
static int get_bom_vector(Vector* bom_vector) {
155

  
156
	return EXIT_SUCCESS;
157

  
158
}
trunk/code/behaviors/formation_control/push_pull/push_pull.c
1
#include <stdint.h>
2
#include <dragonfly_lib.h>
3
#include <wl_basic.h>
4
#include <stdlib.h>
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(100 * 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
	100,
33
	92,
34
	71,
35
	38,
36
	0,
37
	-38,
38
	-71,
39
	-92,
40
	-100,
41
	-92,
42
	-71,
43
	-38,
44
	0,
45
	38,
46
	71,
47
	92
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(100 * 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
	38,
64
	71,
65
	92,
66
	100,
67
	92,
68
	71,
69
	38,
70
	0,
71
	-38,
72
	-71,
73
	-92,
74
	-100,
75
	-92,
76
	-71,
77
	-38
78
};
79

  
80

  
81

  
82
int main (void) {
83

  
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};
86

  
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
	dragonfly_init(ALL_ON);
107
	xbee_init();
108
	encoders_init();
109

  
110
	orbs_set_color(BLUE, GREEN);
111
	delay_ms(1000);
112
	orbs_set_color(GREEN, BLUE);
113
	delay_ms(1000);
114
	orbs_set_color(RED, RED);
115

  
116
	while (1) {
117

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

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

  
134
		if (total_intensity > 0) {
135
			normalized_net_x_comp = net_x_comp / total_intensity;
136
			normalized_net_y_comp = net_y_comp / total_intensity;
137
		}
138

  
139
		usb_puts("x: ");	
140
		usb_puti(normalized_net_x_comp);
141
		usb_puts("\ty: ");
142
		usb_puti(normalized_net_y_comp);
143
		usb_puts("\n");
144

  
145
		delay_ms(50);
146

  
147
	}
148

  
149
	while(1);
150

  
151
}
152

  
153

  
154
static int get_bom_vector(Vector* bom_vector) {
155

  
156
	return EXIT_SUCCESS;
157

  
158
}

Also available in: Unified diff