/ - Diff - Colony - Robotics Club

     /** This test should just have the robot stay in place,
      * register the token ring, participate in the token ring,
      * and reply to packets when asked by the computer.*/
      * and send packets after flashing its BOM*/
     #include <dragonfly_lib.h>
     #include <wireless.h>
-...
         //odometry_init();
     	wl_init(); usb_puts("wireless turned on\r\n");
         wl_set_channel(WL_CHANNEL);
         wl_register_packet_group(&requestHandler);
         	wl_set_channel(WL_CHANNEL);
         	wl_register_packet_group(&requestHandler);
         wl_token_ring_register();
         	wl_token_ring_register();
     	wl_token_ring_set_bom_functions(transmit, bom_off, get_max_bom);
         wl_token_ring_join();
         	wl_token_ring_join();
     	usb_puts("token ring joined\r\n");
     	char buf[500];
     	int prescalar = 0;
         while(1)
+        {
         	while(1)
+        	{
     		wl_do();
     		if(!(++prescalar%1000)){
-...
     		int store[9];
     		store[0] = 0;
         	store[1] = 0;
         	*((double*)(store + 2)) = 5.0;
         	store[4] = 125;	/* IR1 range*/
         		store[1] = 0;
         		*((double*)(store + 2)) = 5.0;
         		store[4] = 125;	/* IR1 range*/
     		store[5] = 125;	// IR2 range
     		store[6] = 125;	// IR3 range
     		store[7] = 125;	// IR4 range
-...
     		wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP,
     			DATA_POINT, data, 18, source, 0);
     		delay_ms(50);
+    	}
     	else if(type==DATA_SERVER_REPLY){
     		server = source;
-...
     	if(server == -1){
     		wl_send_global_packet(MAP_REQUEST_GROUP,
     			DATA_SERVER_IDENTIFY, NULL, 0, 0);
     		delay_ms(50);
+    	}
     	else{
     		int store[9];
     		store[0] = 0; //X
         	store[1] = 0; //Y
         	*((double*)(store + 2)) = 5.0; //Theta.
         	store[4] = 125;	// IR1 range
         		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
-...
     		wl_send_robot_to_robot_global_packet(MAP_REQUEST_GROUP,
     			DATA_POINT, data, 18, server, 0);
     		delay_ms(50);
     		bom_on();
+    	}

     %loop through each line of file
     i = 1; ir = 1;
     %for i=1:size(Data, 1)
     for i=1:size(Data, 1)
         row = Data(i, :);
         x = row(1);
         y = row(2);
         theta = row(3);
     	%look at each IR sensor reading
         %for ir=1:5
             %if row(ir+3) ~= -1
         for ir=1:5
             if row(ir+3) ~= -1
                 %Find vector of object relative to center of bot
                 distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm
                 ObjectVector = distance * Angle_IR(ir, :);
-...
                 objTheta = objTheta + theta;
                 %add absolute position and shift into image
                 objX = (objR * cos(objTheta)) + x + 1500;
                 objY = (objR * sin(objTheta)) + y + 1500;
                 objX = (objR * cos(objTheta)) + x + size(X1,1);
                 objY = (objR * sin(objTheta)) + y + size(X2,2);
                 ObjectVector = [objX, objY];
     			%Calculate distribution for this point:
     			%Calculate distribution for this data point:
     			%Range variance in y direction, rotated by the object angle.
     			rotation = [cos(objTheta),sin(objTheta); -sin(objTheta),cos(objTheta)];
-...
     			%(TODO introduce error in position estimation perpendicular to the robot)
     			rotation = [cos(theta),sin(theta); -sin(theta),cos(theta)];
     			position_error_cov = inv(rotation)*[0,0;0,pos_error]*rotation;
     			covar = position_error_cov + range_error_cov;
     			dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2))
     			surf(X1,X2,dist);
     			colormap default;
     			return;
     			dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2));
     			%Using the values of the pdf as an approximation for the prob. density map
     			%Using the values of the pdf as an approximation for the prob. density map NOT SAFE
     			%(needs to change) update as:
     			prob_map = prob_map.*(1-dist) + dist;
             %end
         %end
     %end
             end
         end
     end
     %graph the probability map.
     surf(X1,X2,prob_map);
     colormap default;

     PacketGroupHandler requestHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
     struct robot_datapoint{
     	time_t time;
     	time_t time; //TODO write time too.
     	short number;
     	short range[5];
     	short x;
-...
+    		{
     			double** point, **ranges, *theta, *robot;
     			int num_buffer_elements = tail_index - head_index;
     			if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;
     			int num_buffer_elements = tail_index >= head_index ?
     				tail_index - head_index : tail_index - head_index + BUFFER_SIZE;
     			ret[0] = mxCreateDoubleMatrix(num_buffer_elements, 2, mxREAL);
     			ret[1] = mxCreateDoubleMatrix(num_buffer_elements, 5, mxREAL);
-...
     			robot = mxGetPr(ret[3]);
     			for(index = 0; index < num_buffer_elements; index++){
     				int target = (head_index + index) % BUFFER_SIZE;
     				int target = (head_index + index) % BUFFER_SIZE;
     				point[i][0] = (double)robot_data[target].x;
     				point[i][1] = (double)robot_data[target].y;
-...
     		short x,y,i;
     		short ranges[5];
     		struct robot_datapoint data;
     		/*Bytes 0-3*/
     		x = ((short)packet[1] << 8)  | (short)packet[0];
     		y = ((short)packet[3] << 8)  | (short)packet[2];
     		data.x = ((short)packet[1] << 8)  | (short)packet[0];
     		data.y = ((short)packet[3] << 8)  | (short)packet[2];
     		/*Bytes 4-7*/
     		float theta = *((float*)(packet + 4));
     		data.orientation = *((float*)(packet + 4));
     		/*Bytes 8-17*/
     		for(i=0; i<5; i++){
     			ranges[i]
     			data.range[i]
     			   = ((short)packet[8+2*i+1] << 8) | (short)packet[8 + 2*i];
+    		}
     		/*Write new data to buffers. (Lock down!)*/
     		pthread_rwlock_wrlock(&buffer_lock);
+    		{
     			/*Note that you write no matter what.
     			* Also, if the buffer is empty, tail_index becomes 0*/
     			tail_index = (tail_index + 1) % BUFFER_SIZE;
     			/*Write the robots number.*/
     			robot_data[tail_index].number = source;
-...
     			/*If the oldest member didn't exist, buffer was empty.*/
     			if(head_index == -1) head_index = 0;
     			/*Otherwise check for overwritten data.	*/
     			/*Otherwise check for overwritten data.*/
     			else if(tail_index == head_index)
     				head_index = (head_index + 1) % BUFFER_SIZE;
+    		}
     		pthread_rwlock_unlock(&buffer_lock);
+    	}
     	else if(type == DATA_SERVER_IDENTIFY){
     		/*Sleep before sending an identity packet.*/
     		printf("Announcing self as server\n");
     		usleep(5000);
     		wl_send_global_packet(MAP_REQUEST_GROUP, DATA_SERVER_REPLY, NULL, 0, 0);
     		usleep(5000);
+    	}
+    }

Project

General

Profile

Colony

Revision 1361