Colony

#include <stdio.h>

#include <stdlib.h>

//#include <mex.h>

#include <pthread.h>

#include <unistd.h>

#include "../../libwireless/lib/wireless.h"

#include "../../libwireless/lib/wl_token_ring.h"

#define WL_COM_PORT "/dev/ttyUSB1" 

#define WL_CHANNEL 0xE

#define MAP_RECEIVE_GROUP 1

#define MAP_REQUEST_GROUP 2

#define POINT_RAW 1 // packet type for map data points w/ raw encoder data

#define POINT_ODO 2 // packet type for map data points w/ odometry data

#define BUFFER_SIZE 1000

#define BUFFER_EMPTY -1

pthread_rwlock_t buffer_lock;

pthread_t receive_thread;

int packets_received; //Used to block overlapping packet requests.

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

PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};

PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL};

//Buffers

short robot_buffer[BUFFER_SIZE];			//by #

short range_buffer[BUFFER_SIZE][5]; 		//IR1, IR2 ... 

short coordinate_buffer[BUFFER_SIZE][2];	//X Y

float orientation_buffer[BUFFER_SIZE];

int tail_index; //Points to the newest data in the buffers.

int head_index; //Points to the oldest data in the buffers.

int receive_initialized = 0;

void* buffer_data(void* arg);

//void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){

int main(){	

	if(!receive_initialized){	

		//Initialization

		head_index = BUFFER_EMPTY;

		tail_index = BUFFER_EMPTY;	

		pthread_rwlock_init(&buffer_lock, NULL);

		pthread_create(&receive_thread, NULL, buffer_data, NULL);

		receive_initialized = 1;

	}

	/*Return stuff to matlab

	//deliberately ignore nrhs.	

	if(nlhs != 4){

		mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n 

					  where P = [X Y], IR = [IR1, IR2 ...],\n

					  T = angle, and R = Robot number."); 

	}

	double* point, ranges, theta;

	int index,i,j;

	pthread_rwlock_rdlock(&buffer_lock);

	{

		if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){

			//Appropriately return with nothing.

		}

		else{

			//TODO Check for the 'already empty' case.

			int num_buffer_elements = tail_index - head_index;

			if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;	

			ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL);

			ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL);

			ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);

			ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);

			point  = mxGetPr(ret[0]);

			ranges = mxGetPr(ret[1]);

			theta = mxGetPr(ret[2]);

			robot = mxGetPr(ret[3]);

			for(index = head_index; index <= tail_index; index = (index + 1)%1024){

				point[i][0] = (double)coordinate_buffer[index][0];	

				point[i][1] = (double)coordinate_buffer[index][1];	

				for(j=0;j<5;j++)

					ranges[i][j] = (double)range_buffer[index][j];

				theta[i] = (double)(orientation_buffer[index]);	

				robot[i] = (double)(robot_buffer[index]);

			}

			head_index = BUFFER_EMPTY;

			tail_index = BUFFER_EMPTY;

	}	

	pthread_rwlock_unlock(&buffer_lock);

	*/

	return -1;

}

/*

 * buffer_data continuously runs through the robots and

 * waits for them to respond.

 */

void* buffer_data(void* arg){

	int robot_num, num_robots;	

	pthread_detach(pthread_self());

	//General communication initialization.	

	wl_set_com_port(WL_COM_PORT);	

	wl_init();

	wl_set_channel(WL_CHANNEL);

	wl_register_packet_group(&receiveHandler);

	//Token ring / sensor network initialization.

	wl_token_ring_register();	

	//Continuously iterate over all the robots - buffering data.

	while(1){

		wl_token_iterator_begin();

		num_robots = wl_token_get_num_robots();

		//Initialize the 'packets received' number. 

		packets_received = 0;

		//Send a data request to all robots.

		while(wl_token_iterator_has_next()){

			robot_num = wl_token_iterator_next();

			wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 0,

            	NULL, 0, robot_num, 0);

		}

		//Wait for all packets to be received before sending more requests.

		while(packets_received < num_robots 

		   && packets_received < wl_token_get_num_robots()){

			usleep(10000); //10ms

		}

		usleep(10000); 

	}

}

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

	printf("A packet came in!\n");	

	if(type==POINT_ODO){	

		if(length!=18) printf("Packet came without the standard length!\n");

		// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5

		// Pull in data:

		short x,y,i;

		short ranges[5];	

		x = ((short)packet[1] << 8)  | (short)packet[0];

		y = ((short)packet[3] << 8)  | (short)packet[2];

		for(i=0; i<5; i++){

			ranges[i] 

			   = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i];

		}

		float theta = *((float*)(packet + 4));

		//Write new data to buffers. (Lock down!)

		pthread_rwlock_wrlock(&buffer_lock);

		{	

			//Note that you write no matter what.	

			tail_index = (tail_index + 1) % BUFFER_SIZE; 

			//Write the robots number.

			robot_buffer[tail_index] = source;

			//Write in range data.

			for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i];

			coordinate_buffer[tail_index][0] = x;

			coordinate_buffer[tail_index][1] = y;

			orientation_buffer[tail_index] = theta;

			//If the oldest member didn't exist, buffer was empty.

			if(head_index == BUFFER_EMPTY) head_index = 0;

			//Otherwise check for overwritten data.	

			else if(tail_index == head_index) 

				head_index = (head_index +1) % BUFFER_SIZE;  

		}

		pthread_rwlock_unlock(&buffer_lock);

		//Increment for the transmit thread.

		packets_received++;

	}

}

CC= gcc

CFLAGS= -Wall -g

LIBS= -lpthread -lwireless

all: test

test: *.c ../../libwireless/lib/*.c 

	$(CC) $(CFLAGS) *.c -L../../../lib/bin -o test -DWL_DEBUG $(LIBS)

clean:

	rm *.o test ../lib/*.o