root / trunk / code / behaviors / formation_control / push_pull / main.c @ 1510
History | View | Annotate | Download (3.27 KB)
1 | 1510 | jsexton | #include <stdint.h> |
---|---|---|---|
2 | 1156 | deffi | #include <dragonfly_lib.h> |
3 | 1502 | jsexton | #include <wl_basic.h> |
4 | 1510 | jsexton | #include <stdlib.h> |
5 | 1156 | deffi | |
6 | 1508 | jsexton | /* Struct for storing vector components */
|
7 | typedef struct { |
||
8 | 1510 | jsexton | int x;
|
9 | int y;
|
||
10 | 1508 | jsexton | } Vector; |
11 | |||
12 | |||
13 | /* Function Prototypes */
|
||
14 | 1510 | jsexton | static int get_bom_vector(Vector*); |
15 | 1508 | jsexton | |
16 | |||
17 | /*******************************
|
||
18 | * BOM Vector Component Tables *
|
||
19 | *******************************/
|
||
20 | |||
21 | 1502 | jsexton | /*
|
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 | 1508 | jsexton | static const signed int x_comp[16] = { |
32 | 1502 | jsexton | 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 | 1156 | deffi | |
50 | |||
51 | 1502 | jsexton | /*
|
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 | 1156 | deffi | |
80 | 1508 | jsexton | |
81 | |||
82 | 1502 | jsexton | int main (void) { |
83 | 1156 | deffi | |
84 | 1502 | jsexton | /* Store current BOM readings and use them as a weighting factor */
|
85 | 1510 | jsexton | uint8_t intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
86 | 1156 | deffi | |
87 | 1504 | jsexton | /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
|
88 | 1502 | jsexton | * components. Calculated by multiplying the intensity by the x and y
|
89 | 1504 | jsexton | * component respectively (x and y components are stored in the tables
|
90 | * above). */
|
||
91 | 1502 | jsexton | 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 | 1504 | jsexton | /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
|
95 | 1502 | jsexton | * components for the entire robot. */
|
96 | long net_x_comp = 0; |
||
97 | long net_y_comp = 0; |
||
98 | 1158 | deffi | |
99 | 1502 | jsexton | /* 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 | 1156 | deffi | |
104 | 1502 | jsexton | int i = 0; |
105 | 1156 | deffi | |
106 | 1502 | jsexton | dragonfly_init(ALL_ON); |
107 | xbee_init(); |
||
108 | encoders_init(); |
||
109 | 1156 | deffi | |
110 | 1502 | jsexton | 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 | 1156 | deffi | |
116 | 1502 | jsexton | while (1) { |
117 | 1156 | deffi | |
118 | 1503 | jsexton | /* Make sure to clear our accumulators */
|
119 | 1502 | jsexton | net_x_comp = 0;
|
120 | net_y_comp = 0;
|
||
121 | total_intensity = 0;
|
||
122 | 1156 | deffi | |
123 | 1502 | jsexton | bom_refresh(BOM_ALL); |
124 | for (i = 0; i < 16; i++) { |
||
125 | 1504 | jsexton | /* BOM intensity is actually measured as more intense = closer to 0 */
|
126 | 1502 | jsexton | 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 | 1156 | deffi | } |
133 | |||
134 | 1503 | jsexton | 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 | 1156 | deffi | |
139 | 1502 | jsexton | usb_puts("x: ");
|
140 | usb_puti(normalized_net_x_comp); |
||
141 | usb_puts("\ty: ");
|
||
142 | usb_puti(normalized_net_y_comp); |
||
143 | 1503 | jsexton | usb_puts("\n");
|
144 | 1156 | deffi | |
145 | 1508 | jsexton | delay_ms(50);
|
146 | 1503 | jsexton | |
147 | 1502 | jsexton | } |
148 | 1156 | deffi | |
149 | 1502 | jsexton | while(1); |
150 | 1156 | deffi | |
151 | } |
||
152 | 1508 | jsexton | |
153 | |||
154 | 1510 | jsexton | static int get_bom_vector(Vector* bom_vector) { |
155 | 1508 | jsexton | |
156 | 1510 | jsexton | return EXIT_SUCCESS;
|
157 | |||
158 | 1508 | jsexton | } |