Colony

#include <stdlib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include <string.h>

#include <stdio.h>

#include <unistd.h>

#include "robots.h"

#define CHANNEL 0xE

#define GROUP 1

void do_nothing(void);

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

void make_listener()

{

     PacketGroupHandler *receiver;

     printf("wl_init() returned %d\n", wl_init());

     wl_set_channel(CHANNEL);

     receiver = malloc(sizeof(PacketGroupHandler));

     receiver->groupCode = GROUP;

     receiver->handle_response = do_nothing;

     receiver->timeout_handler = do_nothing;

     receiver->handle_receive = packet_receive;

     wl_register_packet_group(receiver);

}

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

{

     /* stolen from cmar */

     char buffer[22];

     short x,y,ir1,ir2,ir3,ir4,ir5, n;

     x = ((short)packet[1] << 8) | (short)packet[0];

     y = ((short)packet[3] << 8) | (short)packet[2];

     ir1 = ((short)packet[9] << 8) | (short)packet[8];

     ir2 = ((short)packet[11] << 8) | (short)packet[10];

     ir3 = ((short)packet[13] << 8) | (short)packet[12];

     ir4 = ((short)packet[15] << 8) | (short)packet[14];

     ir5 = ((short)packet[17] << 8) | (short)packet[16];

     char tarr[] = {packet[4],packet[5],packet[6],packet[7]};

     float *theta_ptr = (float *)tarr;

     float theta = *theta_ptr;

     printf( "%d %d %f %d %d %d %d %d\n", x, y, theta,

            ir1, ir2, ir3, ir4, ir5);

     buffer[0] = (char) source;

     buffer[1] = (char) length;

     buffer[2] = type;

     buffer[3] = 0;

     memcpy ( buffer+4, packet, length);

     n = write(newsockfd, buffer, 22);

     bzero(buffer, 22);

}

void do_nothing()

{

     return;

}