Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <string.h>

#include "global.h"

#include "comm_robot.h"

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

#define received_state_waiting 0

#define received_state_pre_timeout 1

#define received_state_timeout 2

#define received_state_received 3

#define received_state_uninitialized 4

#define MAX_PACKET_LENGTH 32

uint8_t received_buffer[MAX_PACKET_LENGTH]; // Maximum length of a packet to be handled

uint8_t received_length;

uint8_t received_state;

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

// ** Motor direction **

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

/**

  * All the functions in this file use these motor settings: a pair (direction/velocity) maps to a pair motor_direction/

  * motor_velocity. motor_direction and motor_velocity are the values passed to the library functions and also sent

  * to the robot. direction also includes an "off" direction. If the direction is off, the motor_velocity is always 0.\

  **/

char *motor_direction_string (uint8_t direction)

{

        if (direction==motor_direction_forward)

                return "forward";

        else if (direction==motor_direction_backward)

                return "backward";

        else if (direction==motor_direction_off)

                return "off";

        else

                return "?";

}

static uint8_t motor_direction (uint8_t direction, uint8_t velocity)

{

        if (direction==motor_direction_backward)

                return BACKWARD;

        else

                return FORWARD;

}

static uint8_t motor_velocity (uint8_t direction, uint8_t velocity)

{

        if (direction==motor_direction_off)

                return 0;

        else

                return velocity;

}

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

// ** Common comm functions        **

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

static void send_packet (char type, char *data, int len)

{

        // Right now we only wait until we receive *any* packet (with the appropriate group), so the functionality for

        // sending command packets and request packets is the same.

        // Note that this will horribly fail if other people are using the same channel/group.

        // Keep sending the packet until we get a reply (retry on timeout)

        do

        {

                received_state=received_state_waiting;

                // Send the request packet

                wl_send_global_packet (station_robot_group, type, data, len, 0);

                // When the reply is received, the receive handler will set received_state to recieved_state_received. Wait

                // until this happens, periodically calling the wl_do function to handle incoming messages.

                // Keep polling the wireless until the state is either "received" or "timeout" (as set by the callbacks)

                while (received_state!=received_state_received && received_state!=received_state_timeout)

                        wl_do();

                if (received_state==received_state_timeout)

                        usb_puts ("# Timeout, retrying" NL);

        }

        while (received_state==received_state_timeout);

}

// A packet that is followed by a "done" packet from the robot.

static void send_command_packet (char type, char *data, int len)

{

        send_packet (type, data, len);

}

// A packet that is followed by a data packet from the robot

static void send_request_packet(char type, char *data, int len)

{

        send_packet (type, data, len);

}

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

// ** Individual commands **

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

void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)

{

        char data[6];

        data[0]=red1;

        data[1]=green1;

        data[2]=blue1;

        data[3]=red2;

        data[4]=green2;

        data[5]=blue2;

        send_command_packet (station_robot_set_orbs, data, 6);

}

/** Direction is motor_direction_off/forward/backward, not FORWARD/BACKWARD */

void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)

{

        char data[4];

        data[0]=motor_direction (direction1, speed1);

        data[1]=motor_velocity  (direction1, speed1);

        data[2]=motor_direction (direction2, speed2);

        data[3]=motor_velocity  (direction2, speed2);

        send_command_packet (station_robot_set_motors, data, 4);

}

/** Direction is direction_off/forward/backward, not FORWARD/BACKWARD */

void robot_set_motors_time (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2, uint16_t time_ms)

{

        char data[6];

        data[0]=motor_direction (direction1, speed1);

        data[1]=motor_velocity  (direction1, speed1);

        data[2]=motor_direction (direction2, speed1);

        data[3]=motor_velocity  (direction2, speed2);

        data[4]=WORD_BYTE_0(time_ms);

        data[5]=WORD_BYTE_1(time_ms);

        send_command_packet (station_robot_set_motors_time, data, 6);

}

void robot_set_motors_off (void)

{

        send_command_packet (station_robot_set_motors_off, NULL, 0);

}

void robot_set_bom (uint16_t bitmask)

{

        char data[2];

        data[0]=WORD_BYTE_0(bitmask);

        data[1]=WORD_BYTE_1(bitmask);

        send_command_packet (station_robot_set_bom, data, 2);

}

void robot_reset_encoders (void)

{

        send_command_packet (station_robot_reset_encoders, NULL, 0);

}

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

// ** Individual requests **

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

bool robot_read_encoders (int16_t *left, int16_t *right)

{

        send_request_packet (station_robot_read_encoders, NULL, 0);

        // Now the received data is in received_buffer/received_length

        if (received_length>=4)

        {

                *left =WORD(received_buffer[0], received_buffer[1]);

                *right=WORD(received_buffer[2], received_buffer[3]);

                return true;

        }

        else

        {

                return false;

        }

}

bool robot_read_bom (uint8_t num, uint16_t *value)

{

        send_request_packet(station_robot_read_bom, (char *)&num, 1);

        // Now the received data is in received_buffer/received_length

        if(received_length >= 2)

        {

                *value = WORD(received_buffer[0], received_buffer[1]);

                return true;

        }

        else

        {

                return false;

        }

}

bool robot_read_rangefinder (uint8_t num, uint16_t *value)

{

        send_request_packet(station_robot_read_rangefinder, (char *)&num, 1);

        // Now the received data is in received_buffer/received_length

        if(received_length >= 2)

        {

                *value = WORD(received_buffer[0], received_buffer[1]);

                return true;

        }

        else

        {

                return false;

        }

}

bool robot_read_bom_all (int16_t *value)

{

        send_request_packet(station_robot_read_bom_all, NULL, 0);

        // Now the received data is in received_buffer/received_length

        if(received_length >= 32)

        {

                value[0] = WORD(received_buffer[0], received_buffer[1]);

                value[1] = WORD(received_buffer[2], received_buffer[3]);

                value[2] = WORD(received_buffer[4], received_buffer[5]);

                value[3] = WORD(received_buffer[6], received_buffer[7]);

                value[4] = WORD(received_buffer[8], received_buffer[9]);

                value[5] = WORD(received_buffer[10], received_buffer[11]);

                value[6] = WORD(received_buffer[12], received_buffer[13]);

                value[7] = WORD(received_buffer[14], received_buffer[15]);

                value[8] = WORD(received_buffer[16], received_buffer[17]);

                value[9] = WORD(received_buffer[18], received_buffer[19]);

                value[10] = WORD(received_buffer[20], received_buffer[21]);

                value[11] = WORD(received_buffer[22], received_buffer[23]);

                value[12] = WORD(received_buffer[24], received_buffer[25]);

                value[13] = WORD(received_buffer[26], received_buffer[27]);

                value[14] = WORD(received_buffer[28], received_buffer[29]);

                value[15] = WORD(received_buffer[30], received_buffer[31]);

                return true;

        }

        else

        {

                return false;

        }

}

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

// ** Receive callbacks **

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

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

{

        // Dump the raw packet

        //usb_puts ("# [");

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

        //{

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

        //        usb_puti (packet[i]);

        //}

        //usb_putc (']');

        // We received some data from the robot. It is not handled immediately as some function is waiting for it.

        // Note the fact that we received some data

        received_state=received_state_received;

        // The packet buffer might be overwritten after this function returns, so we have to make a copy of the data.

        // First, determine the number of bytes to copy. It is the minimum of the received length and the size of the data

        // buffer the data is copied to. If the received length is greater than the buffer size, print an error message.

        // We could stop here (while (1);), but then any long packet would stop the program, even if it's unrelated to

        // what we're doing.

        received_length=length;

        if (sizeof (received_buffer)<received_length)

        {

                received_length=sizeof (received_buffer);

                usb_puts ("# Error: received packet is too long for allocated buffer." NL);

        }

        // Now copy the data

        memcpy (received_buffer, packet, received_length);

}

static void robot_station_timeout (void)

{

        if      (received_state==received_state_waiting    ) { received_state=received_state_pre_timeout; }

        else if (received_state==received_state_pre_timeout) { received_state=received_state_timeout    ; }

}

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

// ** Initialization **

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

// Must not be a local variable because the library doesn't make a copy of it.

PacketGroupHandler receive_handler={robot_station_group, robot_station_timeout, NULL, &robot_station_receive, NULL};

void comm_robot_init ()

{

        received_state=received_state_uninitialized;

        received_length=0;

        usb_puts("# Initializing robot communication" NL);

        xbee_init ();

        wl_init();

        usb_puts("# Done" NL);

        //PacketGroupHandler xxx_receive_handler = {robot_station_group, NULL, NULL, &robot_station_receive, NULL};

        wl_register_packet_group(&receive_handler);

}