root / trunk / code / behaviors / formation_control / push_pull / main.c @ 1502
History  View  Annotate  Download (2.73 KB)
1 
#include <dragonfly_lib.h> 

2 
#include <wl_basic.h> 
3  
4 
/*

5 
* The x component of each BOM detector (indexed from 0 to 15)

6 
* was calculated using the following formula:

7 
*

8 
* x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) )

9 
*

10 
* If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,

11 
* this effectively calculates the x component of the emitter vector where

12 
* emitter 0 corresponds to an angle of 0 radians, 4 > pi/2, 8 > pi, ect.

13 
*/

14 
static signed int x_comp[16] = { 
15 
100,

16 
92,

17 
71,

18 
38,

19 
0,

20 
38,

21 
71,

22 
92,

23 
100,

24 
92,

25 
71,

26 
38,

27 
0,

28 
38,

29 
71,

30 
92

31 
}; 
32  
33  
34 
/*

35 
* The y component of each BOM detector (indexed from 0 to 15)

36 
* was calculated using the following formula:

37 
*

38 
* y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) )

39 
*

40 
* If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,

41 
* this effectively calculates the y component of the emitter vector where

42 
* emitter 0 corresponds to an angle of 0 radians, 4 > pi/2, 8 > pi, ect.

43 
*/

44 
static signed int y_comp[16] = { 
45 
0,

46 
38,

47 
71,

48 
92,

49 
100,

50 
92,

51 
71,

52 
38,

53 
0,

54 
38,

55 
71,

56 
92,

57 
100,

58 
92,

59 
71,

60 
38

61 
}; 
62  
63 
int main (void) { 
64  
65 
/* Store current BOM readings and use them as a weighting factor */

66 
int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 
67  
68 
/* Arrays for storing the weighted x ("Forwardness") and y ("Rightedness")

69 
* components. Calculated by multiplying the intensity by the x and y

70 
* component respectively (stored in the tables above). */

71 
int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 
72 
int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; 
73 

74 
/* Accumulators to sum up the net x ("Forwardness") and y ("Rightedness")

75 
* components for the entire robot. */

76 
long net_x_comp = 0; 
77 
long net_y_comp = 0; 
78  
79 
/* Variables used to normalize the net component values */

80 
int total_intensity = 0; 
81 
int normalized_net_x_comp = 0; 
82 
int normalized_net_y_comp = 0; 
83  
84 
int i = 0; 
85  
86 
dragonfly_init(ALL_ON); 
87 
xbee_init(); 
88 
encoders_init(); 
89  
90 
orbs_set_color(BLUE, GREEN); 
91 
delay_ms(1000);

92 
orbs_set_color(GREEN, BLUE); 
93 
delay_ms(1000);

94 
orbs_set_color(RED, RED); 
95  
96 
while (1) { 
97  
98 
net_x_comp = 0;

99 
net_y_comp = 0;

100 
total_intensity = 0;

101  
102 
bom_refresh(BOM_ALL); 
103 
for (i = 0; i < 16; i++) { 
104 
intensity[i] = 255  bom_get(i);

105 
weighted_x_comp[i] = intensity[i] * x_comp[i]; 
106 
weighted_y_comp[i] = intensity[i] * y_comp[i]; 
107 
net_x_comp += weighted_x_comp[i]; 
108 
net_y_comp += weighted_y_comp[i]; 
109 
total_intensity += intensity[i]; 
110 
} 
111  
112 
normalized_net_x_comp = net_x_comp / total_intensity; 
113 
normalized_net_y_comp = net_y_comp / total_intensity; 
114  
115 
delay_ms(500);

116  
117 
usb_puts("x: ");

118 
usb_puti(normalized_net_x_comp); 
119 
usb_puts("\ty: ");

120 
usb_puti(normalized_net_y_comp); 
121 
usb_puts("\n\n");

122  
123 
} 
124  
125 
while(1); 
126  
127 
} 