Colony

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

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

 * and send packets after flashing its BOM*/

#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_SERVER_IDENTIFY 5

#define DATA_SERVER_REPLY 4

#define DATA_POINT 2

#define DATA_REQUEST 3

#define WL_CHANNEL 0xE

int state; int velocity;

int server;

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)

{

        server = -1;

    /* initialize components and join the token ring */

    dragonfly_init(RANGE | BOM | COMM);

        usb_init(); usb_puts("usb turned on\r\n");

    //odometry_init();

        wl_init(); usb_puts("wireless turned on\r\n");

            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();

        usb_puts("token ring joined\r\n");

        char buf[500];

        int prescalar = 0;

            while(1)

            {

                wl_do();

                if(!(++prescalar%1000)){

                        sprintf(buf, "# Robots = %d, # Robots in matrix = %d\r\n",

                                wl_token_get_robots_in_ring(), wl_token_get_num_robots());

                        usb_puts(buf);

                }

    }

        usb_puts("Terminating\r\n");

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

                server = source;

        }

}

void transmit(){

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

        if(server == -1){

                wl_send_global_packet(MAP_REQUEST_GROUP,

                        DATA_SERVER_IDENTIFY, NULL, 0, 0);

        }

        else{

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

                bom_on();

        }

}