Colony

#include <dragonfly_lib.h>

#include <wireless.h>

//#include <termios.h>           

#include <stdio.h>

//#include <unistd.h>

#define GROUP 1

#define MOTOR 0

#define ENCODER 1

#define SENDER 1

#define RECEIVER 0

unsigned char data[4];

sentCount = 0;

char position = 0;

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

PacketGroupHandler packetHandler = {GROUP, 0, 0, &packet_receive, 0};

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

        if(type == ENCODER)

        {

                usb_putc(packet[0]);

                usb_putc(packet[1]);

                usb_putc(packet[2]);

                usb_putc(packet[3]);

        }

}

int main (void) {

        dragonfly_init(ALL_ON);

        wl_init();

        lcd_init();

        lcd_puts("STARTING");

        position = usb_getc();

        lcd_puts("position is ");

        lcd_puti(position);

        if(position == SENDER)

        {

                //Read in from server

                lcd_puts("starting init");

                data[0] = usb_getc(); 

                data[1] = usb_getc();

                data[2] = usb_getc();

                data[3] = usb_getc();

                lcd_puts("Done with init\n");

                wl_send_global_packet(GROUP, MOTOR, data, 4, 0);

        }

        else

        {

                wl_register_packet_group(&packetHandler);

        }

//        lcd_clear_screen();

  while (1) {

  if(position == SENDER)

  {

                data[0] = usb_getc(); 

                data[1] = usb_getc();

                data[2] = usb_getc();

                data[3] = usb_getc();

                wl_send_global_packet(GROUP, MOTOR, data, 4, 0);

}        

          wl_do();

  }

}