Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#define GROUP 1

#define MOTOR 0

#define ENCODER 1

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

int left_dir = FORWARD;

int right_dir = FORWARD;

int left_speed = 0;

int right_speed = 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) {

        if (type != MOTOR) return;

        //orb_set_color(WHITE);

        left_dir = packet[LD];

        left_speed = packet[LS];

        right_dir = packet[RD];

        right_speed = packet[RS];

        sprintf(buf, "Received: %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);

        //motor1_set(FORWARD,230);

        //motor2_set(FORWARD,230);

        while (1) {

                int encoder_left = encoder_get_v(LEFT);

                int encoder_right = encoder_get_v(RIGHT);

                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, ENCODER, encoder, 4, 0);

                sprintf(buf, "Sent (%d): %d %d\n", count++, encoder_left, encoder_right);

                usb_puts(buf);

                wl_do();

                motor1_set(left_dir, left_speed);

                motor2_set(right_dir, right_speed);

                delay_ms(100);

        }

}