root / trunk / code / behaviors / formation_control / push_pull / main.c @ 1508
History | View | Annotate | Download (3.22 KB)
| 1 | 1156 | deffi | #include <dragonfly_lib.h> |
|---|---|---|---|
| 2 | 1502 | jsexton | #include <wl_basic.h> |
| 3 | 1508 | jsexton | #include <stdint.h> |
| 4 | 1156 | deffi | |
| 5 | 1508 | jsexton | /* Struct for storing vector components */
|
| 6 | typedef struct { |
||
| 7 | uint8_t x; |
||
| 8 | uint8_t y; |
||
| 9 | } Vector; |
||
| 10 | |||
| 11 | |||
| 12 | /* Function Prototypes */
|
||
| 13 | static const Vector get_bom_vector(); |
||
| 14 | |||
| 15 | |||
| 16 | /*******************************
|
||
| 17 | * BOM Vector Component Tables *
|
||
| 18 | *******************************/
|
||
| 19 | |||
| 20 | 1502 | jsexton | /*
|
| 21 | * The x component of each BOM detector (indexed from 0 to 15)
|
||
| 22 | * was calculated using the following formula:
|
||
| 23 | *
|
||
| 24 | * x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) )
|
||
| 25 | *
|
||
| 26 | * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
|
||
| 27 | * this effectively calculates the x component of the emitter vector where
|
||
| 28 | * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
|
||
| 29 | */
|
||
| 30 | 1508 | jsexton | static const signed int x_comp[16] = { |
| 31 | 1502 | jsexton | 100,
|
| 32 | 92,
|
||
| 33 | 71,
|
||
| 34 | 38,
|
||
| 35 | 0,
|
||
| 36 | -38,
|
||
| 37 | -71,
|
||
| 38 | -92,
|
||
| 39 | -100,
|
||
| 40 | -92,
|
||
| 41 | -71,
|
||
| 42 | -38,
|
||
| 43 | 0,
|
||
| 44 | 38,
|
||
| 45 | 71,
|
||
| 46 | 92
|
||
| 47 | }; |
||
| 48 | 1156 | deffi | |
| 49 | |||
| 50 | 1502 | jsexton | /*
|
| 51 | * The y component of each BOM detector (indexed from 0 to 15)
|
||
| 52 | * was calculated using the following formula:
|
||
| 53 | *
|
||
| 54 | * y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) )
|
||
| 55 | *
|
||
| 56 | * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space,
|
||
| 57 | * this effectively calculates the y component of the emitter vector where
|
||
| 58 | * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.
|
||
| 59 | */
|
||
| 60 | static signed int y_comp[16] = { |
||
| 61 | 0,
|
||
| 62 | 38,
|
||
| 63 | 71,
|
||
| 64 | 92,
|
||
| 65 | 100,
|
||
| 66 | 92,
|
||
| 67 | 71,
|
||
| 68 | 38,
|
||
| 69 | 0,
|
||
| 70 | -38,
|
||
| 71 | -71,
|
||
| 72 | -92,
|
||
| 73 | -100,
|
||
| 74 | -92,
|
||
| 75 | -71,
|
||
| 76 | -38
|
||
| 77 | }; |
||
| 78 | 1156 | deffi | |
| 79 | 1508 | jsexton | |
| 80 | |||
| 81 | 1502 | jsexton | int main (void) { |
| 82 | 1156 | deffi | |
| 83 | 1502 | jsexton | /* Store current BOM readings and use them as a weighting factor */
|
| 84 | int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
||
| 85 | 1156 | deffi | |
| 86 | 1504 | jsexton | /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")
|
| 87 | 1502 | jsexton | * components. Calculated by multiplying the intensity by the x and y
|
| 88 | 1504 | jsexton | * component respectively (x and y components are stored in the tables
|
| 89 | * above). */
|
||
| 90 | 1502 | jsexton | int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
| 91 | int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
||
| 92 | |||
| 93 | 1504 | jsexton | /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")
|
| 94 | 1502 | jsexton | * components for the entire robot. */
|
| 95 | long net_x_comp = 0; |
||
| 96 | long net_y_comp = 0; |
||
| 97 | 1158 | deffi | |
| 98 | 1502 | jsexton | /* Variables used to normalize the net component values */
|
| 99 | int total_intensity = 0; |
||
| 100 | int normalized_net_x_comp = 0; |
||
| 101 | int normalized_net_y_comp = 0; |
||
| 102 | 1156 | deffi | |
| 103 | 1502 | jsexton | int i = 0; |
| 104 | 1156 | deffi | |
| 105 | 1502 | jsexton | dragonfly_init(ALL_ON); |
| 106 | xbee_init(); |
||
| 107 | encoders_init(); |
||
| 108 | 1156 | deffi | |
| 109 | 1502 | jsexton | orbs_set_color(BLUE, GREEN); |
| 110 | delay_ms(1000);
|
||
| 111 | orbs_set_color(GREEN, BLUE); |
||
| 112 | delay_ms(1000);
|
||
| 113 | orbs_set_color(RED, RED); |
||
| 114 | 1156 | deffi | |
| 115 | 1502 | jsexton | while (1) { |
| 116 | 1156 | deffi | |
| 117 | 1503 | jsexton | /* Make sure to clear our accumulators */
|
| 118 | 1502 | jsexton | net_x_comp = 0;
|
| 119 | net_y_comp = 0;
|
||
| 120 | total_intensity = 0;
|
||
| 121 | 1156 | deffi | |
| 122 | 1502 | jsexton | bom_refresh(BOM_ALL); |
| 123 | for (i = 0; i < 16; i++) { |
||
| 124 | 1504 | jsexton | /* BOM intensity is actually measured as more intense = closer to 0 */
|
| 125 | 1502 | jsexton | intensity[i] = 255 - bom_get(i);
|
| 126 | weighted_x_comp[i] = intensity[i] * x_comp[i]; |
||
| 127 | weighted_y_comp[i] = intensity[i] * y_comp[i]; |
||
| 128 | net_x_comp += weighted_x_comp[i]; |
||
| 129 | net_y_comp += weighted_y_comp[i]; |
||
| 130 | total_intensity += intensity[i]; |
||
| 131 | 1156 | deffi | } |
| 132 | |||
| 133 | 1503 | jsexton | if (total_intensity > 0) { |
| 134 | normalized_net_x_comp = net_x_comp / total_intensity; |
||
| 135 | normalized_net_y_comp = net_y_comp / total_intensity; |
||
| 136 | } |
||
| 137 | 1156 | deffi | |
| 138 | 1502 | jsexton | usb_puts("x: ");
|
| 139 | usb_puti(normalized_net_x_comp); |
||
| 140 | usb_puts("\ty: ");
|
||
| 141 | usb_puti(normalized_net_y_comp); |
||
| 142 | 1503 | jsexton | usb_puts("\n");
|
| 143 | 1156 | deffi | |
| 144 | 1508 | jsexton | delay_ms(50);
|
| 145 | 1503 | jsexton | |
| 146 | 1502 | jsexton | } |
| 147 | 1156 | deffi | |
| 148 | 1502 | jsexton | while(1); |
| 149 | 1156 | deffi | |
| 150 | } |
||
| 151 | 1508 | jsexton | |
| 152 | |||
| 153 | static Vector get_bom_vector() {
|
||
| 154 | |||
| 155 | } |