Statistics
| Revision:

## root / trunk / code / behaviors / formation_control / push_pull / main.c @ 1508

 1 ```#include ``` ```#include ``` ```#include ``` ```/* Struct for storing vector components */ ``` ```typedef struct { ``` ``` uint8_t x; ``` ``` uint8_t y; ``` ```} Vector; ``` ```/* Function Prototypes */ ``` ```static const Vector get_bom_vector(); ``` ```/******************************* ``` ``` * BOM Vector Component Tables * ``` ``` *******************************/ ``` ```/* ``` ``` * The x component of each BOM detector (indexed from 0 to 15) ``` ``` * was calculated using the following formula: ``` ``` * ``` ``` * x_comp[i] = round(100 * cos ( 2 * pi / 16 * i) ) ``` ``` * ``` ``` * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space, ``` ``` * this effectively calculates the x component of the emitter vector where ``` ``` * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect. ``` ``` */ ``` ```static const signed int x_comp = { ``` ``` 100, ``` ``` 92, ``` ``` 71, ``` ``` 38, ``` ``` 0, ``` ``` -38, ``` ``` -71, ``` ``` -92, ``` ``` -100, ``` ``` -92, ``` ``` -71, ``` ``` -38, ``` ``` 0, ``` ``` 38, ``` ``` 71, ``` ``` 92 ``` ```}; ``` ```/* ``` ``` * The y component of each BOM detector (indexed from 0 to 15) ``` ``` * was calculated using the following formula: ``` ``` * ``` ``` * y_comp[i] = round(100 * sin ( 2 * pi / 16 * i) ) ``` ``` * ``` ``` * If the BOM detectors were superimposed onto a 2 dimensional Cartesian space, ``` ``` * this effectively calculates the y component of the emitter vector where ``` ``` * emitter 0 corresponds to an angle of 0 radians, 4 -> pi/2, 8 -> pi, ect. ``` ``` */ ``` ```static signed int y_comp = { ``` ``` 0, ``` ``` 38, ``` ``` 71, ``` ``` 92, ``` ``` 100, ``` ``` 92, ``` ``` 71, ``` ``` 38, ``` ``` 0, ``` ``` -38, ``` ``` -71, ``` ``` -92, ``` ``` -100, ``` ``` -92, ``` ``` -71, ``` ``` -38 ``` ```}; ``` ```int main (void) { ``` ``` /* Store current BOM readings and use them as a weighting factor */ ``` ``` int intensity = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` /* Arrays for storing the weighted x ("Rightness") and y ("Forwardness") ``` ``` * components. Calculated by multiplying the intensity by the x and y ``` ``` * component respectively (x and y components are stored in the tables ``` ``` * above). */ ``` ``` int weighted_x_comp = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` int weighted_y_comp = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` ``` ``` /* Accumulators to sum up the net x ("Rightness") and y ("Forwardness") ``` ``` * components for the entire robot. */ ``` ``` long net_x_comp = 0; ``` ``` long net_y_comp = 0; ``` ``` /* Variables used to normalize the net component values */ ``` ``` int total_intensity = 0; ``` ``` int normalized_net_x_comp = 0; ``` ``` int normalized_net_y_comp = 0; ``` ``` int i = 0; ``` ``` dragonfly_init(ALL_ON); ``` ``` xbee_init(); ``` ``` encoders_init(); ``` ``` orbs_set_color(BLUE, GREEN); ``` ``` delay_ms(1000); ``` ``` orbs_set_color(GREEN, BLUE); ``` ``` delay_ms(1000); ``` ``` orbs_set_color(RED, RED); ``` ``` while (1) { ``` ``` /* Make sure to clear our accumulators */ ``` ``` net_x_comp = 0; ``` ``` net_y_comp = 0; ``` ``` total_intensity = 0; ``` ``` bom_refresh(BOM_ALL); ``` ``` for (i = 0; i < 16; i++) { ``` ``` /* BOM intensity is actually measured as more intense = closer to 0 */ ``` ``` intensity[i] = 255 - bom_get(i); ``` ``` weighted_x_comp[i] = intensity[i] * x_comp[i]; ``` ``` weighted_y_comp[i] = intensity[i] * y_comp[i]; ``` ``` net_x_comp += weighted_x_comp[i]; ``` ``` net_y_comp += weighted_y_comp[i]; ``` ``` total_intensity += intensity[i]; ``` ``` } ``` ``` if (total_intensity > 0) { ``` ``` normalized_net_x_comp = net_x_comp / total_intensity; ``` ``` normalized_net_y_comp = net_y_comp / total_intensity; ``` ``` } ``` ``` usb_puts("x: "); ``` ``` usb_puti(normalized_net_x_comp); ``` ``` usb_puts("\ty: "); ``` ``` usb_puti(normalized_net_y_comp); ``` ``` usb_puts("\n"); ``` ``` delay_ms(50); ``` ``` } ``` ``` while(1); ``` ```} ``` ```static Vector get_bom_vector() { ``` ```} ```