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>

#include "odometry.h"

#define MAP_REQUEST_GROUP 23

#define DATA_POINT 2 

#define DATA_REQUEST 3

int state;

int velocity;

char buf[100];

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

PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, 

        NULL, NULL, respond, NULL};

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(ORB | RANGE | BOM | COMM);

    odometry_init();

        wl_init();

    wl_set_channel(0xE);

    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) {

           if(type==DATA_REQUEST){ 

                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_REQUEST_GROUP, 

                        DATA_POINT, &store, 0, source, 0);

        }

}