Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <string.h>

#include "global.h"

#include "comm_robot.h"

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

//char new_data = 0;

//char finished = 0;

//char data_received[10];

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

uint8_t received_length;

bool received_data;

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

// ** 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        **

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

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

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

{

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

        // TODO: is it guaranteed that the packet will be sent on the next call to wl_do, or

        // do we have to call wl_do in a loop?

        // TODO: wait for "done" from robot.

        wl_do ();

}

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

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

{

        received_data=false;

        // 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_data to the buffer containing the data. Wait

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

        // FIXME pretend that we received the data right away

//        received_data=true;

//        received_length=0;

        while(!received_data)

        {

                wl_do();

        }

}

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

// ** 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, 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_data/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;

        }

}

uint16_t robot_read_bom (uint8_t num)

{

        // FIXME implement

        return 0;

}

uint16_t robot_read_rangefinder (uint8_t num)

{

        // FIXME implement

        return 0;

}

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

// ** Receive callbacks **

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

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

{

        // Dump the raw packet

        //usb_putc ('[');

        //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.

        // TODO introduce a separate packet group for done/data/commands (commands are things to be handled immediately).

        // Note the fact that we received some data

        received_data=true;

        // 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.

        received_length=length;

        if (sizeof (received_buffer)<received_length) received_length=sizeof (received_buffer);

        // Now copy the data

        memcpy (received_buffer, packet, received_length);

}

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

// ** Initialization **

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

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

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

void comm_robot_init ()

{

        received_data=NULL;

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

}