Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include "robot_comm.h"

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

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

//{

//

//}

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

}

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

{

        char data[4];

        data[0]=direction1;

        data[1]=speed1;

        data[2]=direction2;

        data[3]=speed2;

        send_command_packet (station_robot_set_motors, data, 4);

}

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]=direction1;

        data[1]=speed1;

        data[2]=direction2;

        data[3]=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_read_encoders ()

//{

//        send_request_packet (station_robot_read_enocders, NULL, 0);

//}