Colony

#include <stdint.h>

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <stdlib.h>

/* Struct for storing vector components */

typedef struct {

	int x;

	int y;

} Vector;

/* Function Prototypes */

static int get_bom_vector(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 */

	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_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 int get_bom_vector(Vector* bom_vector) {

	return EXIT_SUCCESS;

}