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

        }

}

static void message_reset_encoders (int length, uint8_t *data)

{

        SYNC

        {

                encoder_rst_dx (LEFT);

                encoder_rst_dx (RIGHT);

        }

}

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

// ** Request handlers **

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

static void message_read_encoders (int length, uint8_t *data)

{

        int16_t left, right;

        SYNC

        {

                left = encoder_get_dx (LEFT);

                right = encoder_get_dx (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);

}

static void message_read_rangefinder (int length, uint8_t *data)

{

        int16_t value;

        int rangefinder_id = 0;

        switch(*data)

        {

                case 0: rangefinder_id = IR1; break;

                case 1: rangefinder_id = IR2; break;

                case 2: rangefinder_id = IR3; break;

                case 3: rangefinder_id = IR4; break;

                case 4: rangefinder_id = IR5; break;

        }

        value = range_read_distance(rangefinder_id);

        char senddata[2];

        senddata[0] = WORD_BYTE_0 (value);

        senddata[1] = WORD_BYTE_1 (value);

        wl_send_global_packet (robot_station_group, robot_station_data_rangefinder, senddata, 2, 0);

}

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

        int16_t value;

        bom_refresh(BOM_ALL);

        value = bom_get((*data));

        char senddata[2];

        senddata[0] = WORD_BYTE_0 (value);

        senddata[1] = WORD_BYTE_1 (value);

        wl_send_global_packet (robot_station_group, robot_station_data_bom, senddata, 2, 0);

}

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

        int i;

        int16_6 value[16];

        char senddata[32];

        bom_refresh(BOM_ALL);

        for(i=0; i<16; i++){

                value[i] = bom_get(i);

                senddata[2*i] = wORD_BYTE_0 (value[i]);

                senddata[(2*i)+1] = WORD_BYTE_1 (value[i]);

        }

        wl_send_global_packet(robot_station_group, robot_station_data_bom_all, senddata, 32, 0);

}

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

// ** Message acknowledging **

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

static void send_done (void)

{

        wl_send_global_packet (robot_station_group, robot_station_done, NULL, 0, 0);

}

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

// ** Receive callbacks **

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

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

{

        orb_set (0, 0, 255);

        usb_puti (type);

        usb_puts (" [");

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

        {

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

                usb_puti (packet[i]);

        }

        usb_puts ("]\r\n");

        switch (type)

        {

                // Command messages

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

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

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

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

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

                case station_robot_reset_encoders:  message_reset_encoders  (length, packet); send_done (); break;

                // Request messages

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

                case station_robot_read_rangefinder: message_read_rangefinder  (length, packet); break;

                case station_robot_read_bom:         message_read_bom                   (length, packet); break;

                case station_robot_read_bom_all:         message_read_bom_all           (length, packet); break;

        }

        orb_set (255, 127, 0);

}

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

// ** Main **

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

int main(void) {

    dragonfly_init(ALL_ON);

        encoders_init ();

        analog_init (ADC_START);

        //bom_init ();

    usb_init ();

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

        orb1_set (255, 127, 0);

        xbee_init ();

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

        }

}