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_encoders, NULL, 0);

        return data_received;

}

void receive_encoders(int* data, int length){

        ((int*)data_received)[0] = data[0];

        ((int*)data_received)[1] = data[1];

}

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:  receive_encoders((int*)packet, length);   break;

        }

        new_data = 1;

}

void comm_robot_init ()

{

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

        wl_register_packet_group(&receive_handler);

}