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

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)

{

        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;

}

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

}

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

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

        data[2]=motor_direction (direction2);

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

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

        data[2]=motor_direction (direction2);

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

}

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

}