Colony

/** This test should just have the robot stay in place,

 * register the token ring, participate in the token ring,

 * and reply to packets when asked by the computer.*/

#include <dragonfly_lib.h>

#include <wireless.h>

#include <encoders.h>

#define MAP_RECEIVE_GROUP 17

#define MAP_REQUEST_GROUP 23

#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

#define WL_CNTL_GROUP 4

int state;

int velocity;

char buf[100];

void respond(char type, int source, unsigned char* packet, int length);

PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, 

        NULL, NULL, NULL, NULL};

PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, 

        NULL, NULL, respond, NULL};

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(ALL_ON);

    odometry_init();

        wl_init();

    wl_set_channel(0xE);

    wl_register_packet_group(&receiveHandler);

    wl_register_packet_group(&requestHandler);

    wl_token_ring_register();

    wl_token_ring_join();

    while(1)

    {

                wl_do();

    }

    wl_terminate();

    return 0;

}

void respond(char type, int source, unsigned char* packet, int length) {

    sprintf(buf, "received packet: %d\n", type);

    usb_puts(buf);

        int store[9];

        store[0] = 0;

    store[1] = 0;

    *((double*)(store + 2)) = 5.0;

    store[4] = 125;        // IR1 range

        store[5] = 125;        // IR2 range

        store[6] = 125;        // IR3 range

        store[7] = 125;        // IR4 range

        store[8] = 125;        // IR5 range

        wl_send_robot_to_robot_global_packet(MAP_RECEIVE_GROUP, 

                POINT_ODO, &store, 0, source, 0);

}