Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

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

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

// **********************

// ** Command handlers **

// **********************

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

        }

}

// **********************

// ** Request handlers **

// **********************

static void message_send_encoders (int length, uint8_t *data)

{

        int16_t left, right;

        SYNC

        {

                left = encoder_read (LEFT);

                right = encoder_read (RIGHT);

        }

        char senddata[4];

        senddata[0]=WORD_BYTE_0 (left);

        senddata[1]=WORD_BYTE_1 (left);

        senddata[2]=WORD_BYTE_0 (right);

        senddata[3]=WORD_BYTE_1 (right);

        wl_send_global_packet (robot_station_group, robot_station_data_encoders, senddata, 4, 0);

}

// ***********************

// ** Receive callbacks **

// ***********************

// TODO: different group for commands and requests

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

{

        // TODO why does it keep receiving 0 messages?

        //if (type==0) return;

        // Dump the raw packet

        usb_puts ("Recvd [");

        for (uint8_t i=0; i<length; ++i)

        {

                if (i!=0) usb_putc (' ');

                usb_puti (packet[i]);

        }

        usb_puts ("]\r\n");

        orb_set (0, 0, 255);

        usb_puts ("Received a message: ");

        usb_puti (type);

        usb_puts ("\r\n");

        switch (type)

        {

                // Command messages

                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;

                // Request messages

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

        }

        orb_set (255, 127, 0);

}

// **********

// ** Main **

// **********

int main(void) {

    dragonfly_init(0);

        encoders_init ();

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

        }

}