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_REQUEST_GROUP 23

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

#define DATA_REQUEST 3

#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 requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, 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.*/

void init_wireless(void);

void* run_wireless(void*);

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

	if(nrhs == 1){	

		/*Initialization*/

		init_wireless();

		pthread_rwlock_init(&buffer_lock, NULL);

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

		return;

	}

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

	}

	int index,i,j;

	pthread_rwlock_rdlock(&buffer_lock);

	{

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

			double* point, *ranges, *theta, *robot;

			/*Appropriately return with nothing. (all 0's)*/

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

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

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

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

			point  = mxGetPr(ret[0]);

			ranges = mxGetPr(ret[1]);

			theta = mxGetPr(ret[2]);

			robot = mxGetPr(ret[3]);

			point[0] = point[1] = 0.0;

			ranges[0] = ranges[1] = ranges[2] = 0.0;

			ranges[3] = ranges[4] = 0.0;

			theta[0] = 0.0;

			robot[0] = 0.0;

			return;

		}

		else{

			double** point, **ranges, *theta, *robot;

			int buf_count, 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);

			/*This may not work on some computers.*/

			point  = (double**)mxGetPr(ret[0]);

			ranges = (double**)mxGetPr(ret[1]);

			theta = mxGetPr(ret[2]);

			robot = mxGetPr(ret[3]);

			for(index = head_index, buf_count = 0; buf_count < num_buffer_elements; 

				buf_count++, 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 = tail_index = BUFFER_EMPTY;

		}

	}	

	pthread_rwlock_unlock(&buffer_lock);

}

void init_wireless(void){

	/*General communication initialization.	*/

	wl_set_com_port(WL_COM_PORT);	

	wl_init();

	wl_set_channel(WL_CHANNEL);

	wl_register_packet_group(&requestHandler);

	/*Token ring / sensor network initialization.*/

	wl_token_ring_register();	

}

void* run_wireless(void* arg){

	pthread_detach(pthread_self());

	while(1){

		wl_do();

		usleep(5000);

	}

}

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

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

	if(type==DATA_POINT){	

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

	}

}

#define WL_COM_PORT "/dev/ttyUSB1" 

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

void* buffer_data(void* arg);

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

			/*Appropriately return with nothing.*/

			return;

		else{

			/*This may not work on some computers.*/

/*

 * buffer_data continuously runs through the robots and

 * waits for them to respond.

void* buffer_data(void* arg){

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

		printf("sending new round of packets\n");	

		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()){

			wl_do();

			robot_num = wl_token_iterator_next();

			printf("sending a packet to robot %d\n",robot_num);

			wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 

				DATA_REQUEST, NULL, 0, robot_num, 0);

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

				wl_do();	

				usleep(10000); /*500ms*/

			}

		}

		time_out = 50; /*50 * 10ms = 1s*/

		/*Wait for all packets to be received 

		 * before sending more requests.

		 * ring etc.) For example, if a robot drops

		 * a packet (highly likely) it is not dealt with

		 * */

		while(packets_received < num_robots 

		   && packets_received < wl_token_get_num_robots() && time_out){

			wl_do();

			time_out--;	

			usleep(10000); /*10ms*/

		}

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

			wl_do();	

			usleep(10000); /*500ms*/

		}

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

		/*Increment for the transmit thread.*/

		packets_received++;

    dragonfly_init(ORB | RANGE | BOM | COMM);

    wl_set_channel(0xE);

			DATA_POINT, &store, 0, source, 0);

TARGET = robot_main

all: receive receive2

receive2:

	 mex -g receive2.c -L../../libwireless/lib -lwireless