Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

#include "../common/comm_station_robot.h"

#include "../../../lib/include/libdragonfly/encoders.h"

static void message_set_orbs (int length, uint8_t *data)

{

        if (length>=6)

        {

                orbs_set (data[0], data[1], data[2], data[3], data[4], data[5]);

        }

}

static void message_set_motors (int length, uint8_t *data)

{

        if (length>=4)

        {

                motor1_set (data[0], data[1]);

                motor2_set (data[2], data[3]);

        }

}

static void message_set_motors_time (int length, uint8_t *data)

{

        if (length>=6)

        {

                motor1_set (data[0], data[1]);

                motor2_set (data[2], data[3]);

                delay_ms (WORD(data[4], data[5]));

                motors_off ();

        }

}

static void message_set_motors_off (int length, uint8_t *data)

{

        motors_off ();

}

static void message_set_bom (int length, uint8_t *data)

{

        if (length>=2)

        {

                uint16_t bitmask=WORD(data[0], data[1]);

                bom_set_leds (bitmask);

                if (bitmask==0)

                        bom_off ();

                else

                        bom_on ();

        }

}

static void message_send_encoders (int length, uint8_t *data){

        int left = encoder_read(LEFT);

        int right = encoder_read(RIGHT);

        int vals[2];

        vals[0] = left;

        vals[1] = right;

        wl_send_global_packet (robot_station_group, robot_station_data_encoders, (char*)vals, 8, 0);

}

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

{

        switch (type)

        {

                case station_robot_set_orbs:        message_set_orbs        (length, packet); break;

                case station_robot_set_motors:      message_set_motors      (length, packet); break;

                case station_robot_set_bom:         message_set_bom         (length, packet); break;

                case station_robot_set_motors_off:  message_set_motors_off  (length, packet); break;

                case station_robot_set_motors_time: message_set_motors_time (length, packet); break;

                case station_robot_read_encoders:        message_send_encoders        (length, packet); break;

        }

}

int main(void) {

    dragonfly_init(ALL_ON);

    usb_init ();

    usb_puts ("Diagnostic station robot starting up\r\n");

    orb_init_pwm ();

        xbee_init ();

        orb1_set (255, 127, 0);

        wl_init();

        orb2_set (255, 127, 0);

        //usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");        // -1

        //usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");        // 0

        //usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");        // 4/8

        PacketGroupHandler receive_handler = {station_robot_group, NULL, NULL, &station_robot_receive, NULL};

        wl_register_packet_group(&receive_handler);

        while (1)

        {

                wl_do();

        }

}