Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#define GROUP 1

#define TYPE 0

#define LS 0

#define LD 1

#define RS 2

#define RD 3

#define H0 0

#define L0 1

#define H1 2

#define L1 3

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

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

char buf[32];

int count = 0;

/* receive motor speed from master: 

 * left_speed left_dir right_speed right_dir

 * then sent raw encoder values back to master: 

 * high_left low_left high_right low_right 

 */

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

        //orb_set_color(WHITE);

        usb_puts("receiving\n");

        sprintf(buf, "%d %d %d %d\n", packet[LS], packet[LD], packet[RS], packet[RD]);

        usb_puts(buf);

        motor1_set(packet[LD],packet[LS]);

        motor2_set(packet[RD],packet[RS]);

        //orb_set_color(GREEN);

}

int main (void) {

        dragonfly_init(ALL_ON);

        usb_init();

        encoders_init();

        //usb_puts("xbee id ");

        //usb_putc(wl_get_xbee_id());

        wl_init();

        wl_register_packet_group(&packetHandler);

        //orb_set_color(RED);

        while (1) {

                wl_do();

                int encoder_left = encoder_read(LEFT);

                int encoder_right = encoder_read(RIGHT);

                sprintf(buf, "%d %d\n", encoder_left, encoder_right);

                usb_puts(buf);

                unsigned char encoder[4];

                encoder[H0] = (encoder_left >> 8) & 0xFF;

                encoder[L0] = (encoder_left) & 0xFF;

                encoder[H1] = (encoder_right >> 8) & 0xFF;

                encoder[L1] = (encoder_right) & 0xFF;

                wl_send_global_packet(GROUP, TYPE, encoder, 4, 0);

                usb_puts("Sent ");

                usb_puti(count++);

                usb_puts("\n");

                delay_ms(200);

        }

}