Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <stdint.h>

/* 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[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 ("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_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() {

}