Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

#include "../common/comm_station_robot.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_bom (int length, uint8_t *data)

{

        // TODO test

        if (length>=2)

        {

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

                bom_set_leds (bitmask);

                if (bitmask==0)

                        bom_off ();

                else

                        bom_on ();

        }

}

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;

                // TODO default: error message, red orbs, pause. Test!

                // TODO: error signal in the individual handlers if the number of parameters is too low. Test!

        }

}

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

        }

}