Colony

#include <dragonfly_lib.h>

#include <wireless.h>

//#include "odometry.h"

#define POLLING_INTERVAL 500	/* interval in ms to get sensor data */

#define MAP 1	// packet group for sending data points

#define POINT_RAW 1 // packet type for map data points w/ raw encoder data

#define POINT_ODO 2 // packet type for map data points w/ odometry data

int main(void)

{

    /* initialize components and join the token ring */

    //wl_init(); 

    //data_response_init();

    //wl_token_ring_register();

    //wl_token_ring_join();

    dragonfly_init(ALL_ON);

    run_around_init();

    //odometry_init();

    //int x, y; // use when sending odometry data

    //float theta;

    // use when sending raw encoder data

    int left_enc, right_enc, range1, range2, range3, range4, range5;

    int store[7];

    char send[8];   // for transmission

    while(1)

    {

	//wl_do();

	run_around_FSM();	/* traverse the environment and avoid obstacles ... */

	delay_ms(POLLING_INTERVAL);	/* until it's time to grab map data */

	//x = odometry_dx(); // use when sending odometry data

	//y = odometry_dy();

	//theta = odometry_angle();

	store[0] = encoder_get_dx(LEFT);    // left encoder distance

	store[1] = encoder_get_dx(RIGHT);   // right encoder distance

	store[2] = range_read_distance(IR1);	// IR1 range

	store[3] = range_read_distance(IR2);	// IR2 range

	store[4] = range_read_distance(IR3);	// IR3 range

	store[5] = range_read_distance(IR4);	// IR4 range

	store[6] = range_read_distance(IR5);	// IR5 range

	//send = (char) store;	// convert to chars for transmission

	// for (int i = 0; i < 7; i++)

	// {

	//     send[i] = (char) store[i];

	// }

	// send[7] = 0;

	// send point w/ raw encoder data

	// wl_send_global_packet(MAP, POINT_RAW, send, 7, 0);

	// send point w/ odometry data

	// wl_send_global_packet(MAP, POINT_ODO, send, 8, 0);

	// send raw encoder data point over serial as a string

	// send[7] = 0;

	// usb_puts(send);	// this is probably ugly, but I don't know any better

	// send raw encoder data point over serial as integers

	for (int i = 0; i < 7; i++)

	{

	    usb_puti(store[i]);

	}

	usb_putc("\n");

    }

    //wl_terminate();

    return 0;