Index: trunk/code/projects/mapping/matlab/receive2.c
===================================================================
--- trunk/code/projects/mapping/matlab/receive2.c	(revision 1027)
+++ trunk/code/projects/mapping/matlab/receive2.c	(nonexistent)
@@ -1,201 +0,0 @@
-#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++;
-	}
-}
-
Index: trunk/code/projects/mapping/matlab/receive.c
===================================================================
--- trunk/code/projects/mapping/matlab/receive.c	(revision 1027)
+++ trunk/code/projects/mapping/matlab/receive.c	(revision 1028)
@@ -12,7 +12,7 @@
 #include "../../libwireless/lib/wireless.h"
 #include "../../libwireless/lib/wl_token_ring.h"
 
-#define WL_COM_PORT "/dev/ttyUSB1" 
+#define WL_COM_PORT "/dev/ttyUSB0" 
 #define WL_CHANNEL 0xE
 
 #define MAP_REQUEST_GROUP 23
@@ -19,6 +19,7 @@
 
 #define DATA_POINT 2 /* packet type for map data points w/ odometry data*/
 #define DATA_REQUEST 3
+#define DATA_INIT 4 /*Just to give the robot a target.*/
 
 #define BUFFER_SIZE 1000
 #define BUFFER_EMPTY -1
@@ -26,8 +27,6 @@
 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};
@@ -41,7 +40,7 @@
 int tail_index; /*Points to the newest data in the buffers.*/
 int head_index; /*Points to the oldest data in the buffers.*/
 
-void* buffer_data(void* arg);
+void* run_wireless(void* arg);
 void init_wireless(void);
 
 void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
@@ -54,7 +53,7 @@
  		
 		pthread_rwlock_init(&buffer_lock, NULL);
 
-		pthread_create(&receive_thread, NULL, buffer_data, NULL);
+		pthread_create(&receive_thread, NULL, run_wireless, NULL);
 		return;
 	}
 	
@@ -74,7 +73,7 @@
 
 		if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
 			double* point, *ranges, *theta, *robot;
-			/*Appropriately return with nothing.*/
+			/*Appropriately return with nothing. (til I figure that out, return 0's)*/
 			ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL);
 			ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL);
 			ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
@@ -93,9 +92,8 @@
 			theta[0] = 0.0;
 			robot[0] = 0.0;
 
-			return;
 		}
-		else{
+		else{ /*Copy the buffer data and set the buffer to empty.*/
 			double** point, **ranges, *theta, *robot;
 			
 			int num_buffer_elements = tail_index - head_index;
@@ -106,7 +104,6 @@
 			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]);
@@ -140,65 +137,22 @@
 	wl_token_ring_register();	
 }
 
-/*
- * buffer_data continuously runs through the robots and
- * waits for them to respond.
+
+/*Call wl_do and occasionally send a broadcast packet declaring the 
+ * number of the server.
  */
-void* buffer_data(void* arg){
+void* run_wireless(void* arg){
 	int robot_num, num_robots,time_out, i;	
 
 	pthread_detach(pthread_self());
 	
 	init_wireless();
+	printf("Wireless initialized\n");
 	
-	/*Continuously iterate over all the robots - buffering data.*/
-	
+	int a = 0;
 	while(1){
+		/*Passively wait for packets to arrive.*/
 		wl_do();
-
-		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*/
-		}
 	}
 }
 
@@ -206,7 +160,8 @@
 	printf("A packet came in!\n");	
 	if(type==DATA_POINT){	
 
-		if(length!=18) printf("Packet came without the standard length!\n");
+		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:*/
@@ -250,9 +205,10 @@
 			
 		}
 		pthread_rwlock_unlock(&buffer_lock);
-		
-		/*Increment for the transmit thread.*/
-		packets_received++;
 	}
+	if(type == DATA_INIT){
+		wl_send_global_packet(MAP_REQUEST_GROUP, DATA_INIT, NULL, 0, 0);
+		printf("Announcing self as server\n");
+	}
 }
 
Index: trunk/code/projects/mapping/matlab/testRobot/test.c
===================================================================
--- trunk/code/projects/mapping/matlab/testRobot/test.c	(revision 1027)
+++ trunk/code/projects/mapping/matlab/testRobot/test.c	(revision 1028)
@@ -4,19 +4,26 @@
 
 #include <dragonfly_lib.h>
 #include <wireless.h>
+#include <wl_token_ring.h>
 #include <encoders.h>
+#include <bom.h>
 #include "odometry.h"
 
 #define MAP_REQUEST_GROUP 23
 
+#define DATA_INIT 4
 #define DATA_POINT 2 
 #define DATA_REQUEST 3
 
+#define WL_CHANNEL = 0xE
+
 int state;
 int velocity;
+int server = -1;
 char buf[100];
 
 void respond(char type, int source, unsigned char* packet, int length);
+void transmit(void);
 
 PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, 
 	NULL, NULL, respond, NULL};
@@ -24,14 +31,15 @@
 int main(void)
 {
     /* initialize components and join the token ring */
-    dragonfly_init(ORB | RANGE | BOM | COMM);
+    dragonfly_init(RANGE | BOM | COMM);
     odometry_init();
     
 	wl_init();
-    wl_set_channel(0xE);
+    wl_set_channel(WL_CHANNEL);
     wl_register_packet_group(&requestHandler);
     
     wl_token_ring_register();
+	wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom);
     wl_token_ring_join();
 
     while(1)
@@ -58,9 +66,37 @@
 		store[7] = 125;	// IR4 range
 		store[8] = 125;	// IR5 range
 
+		char* data = (char*)store;
+
 		wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 
-			DATA_POINT, &store, 0, source, 0);
+			DATA_POINT, data, 18, source, 0);
 	}
+	else if(type==DATA_INIT){
+		server = source;
+	}
 }
 
+void transmit(){
+	bom_on();
 
+	/*If a server hasn't declared itself, return.*/
+	if(server == -1) return;
+	
+	int store[9];
+	store[0] = 0; //X
+    store[1] = 0; //Y
+    *((double*)(store + 2)) = 5.0; //Theta.
+    store[4] = 125;	// IR1 range
+	store[5] = 125;	// IR2 range
+	store[6] = 125;	// IR3 range
+	store[7] = 125;	// IR4 range
+	store[8] = 125;	// IR5 range
+
+	char* data = (char*)store;
+
+	wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP, 
+		DATA_POINT, data, 18, server, 0);
+}
+
+
+
Index: trunk/code/projects/mapping/matlab/testRobot/Makefile
===================================================================
--- trunk/code/projects/mapping/matlab/testRobot/Makefile	(revision 1027)
+++ trunk/code/projects/mapping/matlab/testRobot/Makefile	(revision 1028)
@@ -8,7 +8,7 @@
 endif
 
 # Target file name (without extension).
-TARGET = robot_main
+TARGET = test
 
 # Uncomment this to use the wireless library
 USE_WIRELESS = 1
Index: trunk/code/projects/mapping/matlab/Makefile
===================================================================
--- trunk/code/projects/mapping/matlab/Makefile	(revision 1027)
+++ trunk/code/projects/mapping/matlab/Makefile	(revision 1028)
@@ -2,7 +2,7 @@
 CFLAGS=
 LIBS= -lpthread -lwireless
 
-all: receive receive2
+all: receive
 
 test: *.c ../../libwireless/lib/*.c 
 	$(CC) $(CFLAGS) *.c -L../../../lib/bin -DWL_DEBUG $(LIBS)
@@ -10,9 +10,6 @@
 receive:
 	 mex -g receive.c -L../../libwireless/lib -lwireless
 
-receive2:
-	 mex -g receive2.c -L../../libwireless/lib -lwireless
-
 clean:
 	rm *.o 
 
