root / trunk / code / behaviors / formation_control / push_pull / main.c @ 1504
History | View | Annotate | Download (2.9 KB)
1 | 1156 | deffi | #include <dragonfly_lib.h> |
---|---|---|---|
2 | 1502 | jsexton | #include <wl_basic.h> |
3 | 1156 | deffi | |
4 | 1502 | jsexton | /*
|
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 | 1156 | deffi | |
33 | |||
34 | 1502 | jsexton | /*
|
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 | 1156 | deffi | |
63 | 1502 | jsexton | int main (void) { |
64 | 1156 | deffi | |
65 | 1502 | jsexton | /* 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 | 1156 | deffi | |
68 | 1504 | jsexton | /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
|
69 | 1502 | jsexton | * components. Calculated by multiplying the intensity by the x and y
|
70 | 1504 | jsexton | * component respectively (x and y components are stored in the tables
|
71 | * above). */
|
||
72 | 1502 | jsexton | int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
73 | int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
||
74 | |||
75 | 1504 | jsexton | /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
|
76 | 1502 | jsexton | * components for the entire robot. */
|
77 | long net_x_comp = 0; |
||
78 | long net_y_comp = 0; |
||
79 | 1158 | deffi | |
80 | 1502 | jsexton | /* Variables used to normalize the net component values */
|
81 | int total_intensity = 0; |
||
82 | int normalized_net_x_comp = 0; |
||
83 | int normalized_net_y_comp = 0; |
||
84 | 1156 | deffi | |
85 | 1502 | jsexton | int i = 0; |
86 | 1156 | deffi | |
87 | 1502 | jsexton | dragonfly_init(ALL_ON); |
88 | xbee_init(); |
||
89 | encoders_init(); |
||
90 | 1156 | deffi | |
91 | 1502 | jsexton | orbs_set_color(BLUE, GREEN); |
92 | delay_ms(1000);
|
||
93 | orbs_set_color(GREEN, BLUE); |
||
94 | delay_ms(1000);
|
||
95 | orbs_set_color(RED, RED); |
||
96 | 1156 | deffi | |
97 | 1502 | jsexton | while (1) { |
98 | 1156 | deffi | |
99 | 1503 | jsexton | /* Make sure to clear our accumulators */
|
100 | 1502 | jsexton | net_x_comp = 0;
|
101 | net_y_comp = 0;
|
||
102 | total_intensity = 0;
|
||
103 | 1156 | deffi | |
104 | 1502 | jsexton | bom_refresh(BOM_ALL); |
105 | for (i = 0; i < 16; i++) { |
||
106 | 1504 | jsexton | /* BOM intensity is actually measured as more intense = closer to 0 */
|
107 | 1502 | jsexton | intensity[i] = 255 - bom_get(i);
|
108 | weighted_x_comp[i] = intensity[i] * x_comp[i]; |
||
109 | weighted_y_comp[i] = intensity[i] * y_comp[i]; |
||
110 | net_x_comp += weighted_x_comp[i]; |
||
111 | net_y_comp += weighted_y_comp[i]; |
||
112 | total_intensity += intensity[i]; |
||
113 | 1156 | deffi | } |
114 | |||
115 | 1503 | jsexton | if (total_intensity > 0) { |
116 | normalized_net_x_comp = net_x_comp / total_intensity; |
||
117 | normalized_net_y_comp = net_y_comp / total_intensity; |
||
118 | } |
||
119 | 1156 | deffi | |
120 | 1502 | jsexton | usb_puts("x: ");
|
121 | usb_puti(normalized_net_x_comp); |
||
122 | usb_puts("\ty: ");
|
||
123 | usb_puti(normalized_net_y_comp); |
||
124 | 1503 | jsexton | usb_puts("\n");
|
125 | 1156 | deffi | |
126 | 1503 | jsexton | delay_ms(500);
|
127 | |||
128 | 1502 | jsexton | } |
129 | 1156 | deffi | |
130 | 1502 | jsexton | while(1); |
131 | 1156 | deffi | |
132 | } |