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 132

#define SENDER 1

#define RECEIVER 0

int id = -1;

char data[2];

sentCount = 0;

char position = 0;

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

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

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

        if(type == ENCODER)

        {

                if(id == -1)

                        id = source;

                if(source != id)

                        motor1_set(1, 200);

                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, 2, 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, 2, 0);

        }

          wl_do();

  }

}