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

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

        char buf[32];

        sprintf(buf, "%d %d %d %d\n", packet[0], packet[1], packet[2], packet[3]);

        usb_puts(buf);

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

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

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

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

        }

}