Colony

#include <string.h>

#include "global.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.\

  **/

static uint8_t motor_direction (uint8_t direction, uint8_t velocity)

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

// ** Common comm functions	**

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

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

	received_data=false;

	// Send the request packet

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

	{

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

// ** Individual commands **

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

	data[0]=motor_direction (direction1, speed1);

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

	data[2]=motor_direction (direction2, speed2);

	data[3]=motor_velocity  (direction2, speed2);

	data[0]=motor_direction (direction1, speed1);

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

	data[2]=motor_direction (direction2, speed1);

	data[3]=motor_velocity  (direction2, speed2);

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

// ** Individual requests **

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

bool robot_read_encoders (int16_t *left, int16_t *right)

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

	}

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

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

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

#include <wireless.h>

bool robot_read_encoders (int16_t *left, int16_t *right);

	// Initialize before using USB

	comm_server_init ();

	orb1_set (0, 0, 255);

	comm_robot_init ();

	orb2_set (0, 0, 255);

	int16_t left=69, right=105;

	while (1)

	{

		robot_set_motors (motor_direction_backward, 250, motor_direction_forward, 250);

		delay_ms (200);

	}

	// Encoder test

	while (0)

	{

		bool result=robot_read_encoders (&left, &right);

		usb_puti (result);

		usb_puts (" ");

		usb_puti (left);

		usb_puts (" ");

		usb_puti (right);

		usb_puts (NL);

		delay_ms (200);

	}

	comm_robot_init ();

	return 0;

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

// ** Command handlers **

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

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

// ** Request handlers **

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

static void message_send_encoders (int length, uint8_t *data)

{

	int16_t left, right;

	SYNC

	{

		left = encoder_read (LEFT);

		right = encoder_read (RIGHT);

	}

	char senddata[4];

	senddata[0]=WORD_BYTE_0 (left);

	senddata[1]=WORD_BYTE_1 (left);

	senddata[2]=WORD_BYTE_0 (right);

	senddata[3]=WORD_BYTE_1 (right);

	wl_send_global_packet (robot_station_group, robot_station_data_encoders, senddata, 4, 0);

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

// ** Receive callbacks **

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

// TODO: different group for commands and requests

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

	// TODO why does it keep receiving 0 messages?

	//if (type==0) return;

	// Dump the raw packet

	usb_puts ("Recvd [");

	for (uint8_t i=0; i<length; ++i)

	{

		if (i!=0) usb_putc (' ');

		usb_puti (packet[i]);

	}

	usb_puts ("]\r\n");

	orb_set (0, 0, 255);

	usb_puts ("Received a message: ");

	usb_puti (type);

	usb_puts ("\r\n");

		// Command messages

		// Request messages

	orb_set (255, 127, 0);

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

// ** Main **

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

    dragonfly_init(0);

	encoders_init ();

	usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");	// -1

	usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");	// 0

	usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");	// 4/8