Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include "comm_robot.h"

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

char new_data = 0;

char finished = 0;

char data_received[10];

// 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(char type, char *data, int len)

{

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

        while(!new_data){

                wl_do();

        }

        new_data = 0;

}

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

}

char* robot_read_encoders ()

{

        send_request_packet (station_robot_read_enocders, NULL, 0);

        return data_received;

}

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

{

        switch (type)

        {

                case robot_station_finished:    new_data = 1;    break;

                case robot_station_data_encoders:   new_data = 1;   break;

        }

}