Statistics
| Revision:

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

History | View | Annotate | Download (2.81 KB)

 1 ```#include ``` ```#include ``` ```/* ``` ``` * 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 signed int x_comp[16] = { ``` ``` 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[16] = { ``` ``` 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[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` /* Arrays for storing the weighted x ("Forwardness") and y ("Rightedness") ``` ``` * components. Calculated by multiplying the intensity by the x and y ``` ``` * component respectively (stored in the tables above). */ ``` ``` int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; ``` ``` ``` ``` /* Accumulators to sum up the net x ("Forwardness") and y ("Rightedness") ``` ``` * 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++) { ``` ``` 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(500); ``` ``` } ``` ``` while(1); ``` ```} ```