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 <wl_token_ring.h>

#include <encoders.h>

#include <bom.h>

#include "odometry.h"

#define MAP_REQUEST_GROUP 23

#define DATA_INIT 4

#define DATA_POINT 2 

#define DATA_REQUEST 3

#define WL_CHANNEL = 0xE

int state;

int velocity;

int server = -1;

char buf[100];

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

void transmit(void);

PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, 

        NULL, NULL, respond, NULL};

int main(void)

{

    /* initialize components and join the token ring */

    dragonfly_init(RANGE | BOM | COMM);

    odometry_init();

        wl_init();

    wl_set_channel(WL_CHANNEL);

    wl_register_packet_group(&requestHandler);

    wl_token_ring_register();

        wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom);

    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

                char* data = (char*)store;

                wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 

                        DATA_POINT, data, 18, source, 0);

        }

        else if(type==DATA_INIT){

                server = source;

        }

}

void transmit(){

        bom_on();

        /*If a server hasn't declared itself, return.*/

        if(server == -1) return;

        int store[9];

        store[0] = 0; //X

    store[1] = 0; //Y

    *((double*)(store + 2)) = 5.0; //Theta.

    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

        char* data = (char*)store;

        wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 

                DATA_POINT, data, 18, server, 0);

}