Colony

#include <stdint.h>

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <stdlib.h>

int main (void) {

        /* Store current BOM readings and use them as a weighting factor */

        uint8_t intensity[16] = {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[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 ("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_print_usb(NULL);

                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);

}