Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <encoders.h>

#include "odometry.h"

#include "smart_run_around_fsm.h"

#define POLLING_INTERVAL 50    //interval in ms to get sensor data

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

#define ODO 0        // packet type for sending data points

char buf[100];

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(ALL_ON);

    odometry_init();

    run_around_init();

    orb_enable();

    wl_init();

    wl_set_channel(0xE);

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

    double theta;

    int store[9];

    char *send;   // for transmission

    while(1)

    {

                wl_do();

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

                x = odometry_dx();        // data gathered

                y = odometry_dy();

                theta = odometry_angle();

                store[0] = x;        // position

                store[1] = y;

                *((double*)(store + 2)) = theta;        // calculated angle (8 bytes)

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

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

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

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

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

                sprintf(buf, "%d %d %f %d %d %d %d %d\n", store[0], store[1],

                        *((double *)(store+2)), store[4], store[5], store[6],

                        store[7], store[8]);

                usb_puts(buf);        // send data to USB for debugging

                send = (char*) store;        // convert to char for wireless

                wl_send_global_packet(MAP, ODO, send, 18, 0);

                run_around_FSM();

    }

    wl_terminate();

    return 0;

}