Colony

%Function which creates a map of points seen by a robot

%Input comes from 'filename' where each line contains 8 space-separated values

%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order

%Example line of input file:

%17 23 .45 -1 42 -1 -1 -1

%x and y are centimeters, theta is in radians

%IR values are the values seen by the robot, where -1 means that nothing is seen

%Output: graph of points seen by robot

%All output measurements are in centimeters

%TODO

%find conversion factor between IR sensor values and centimeters (currently 1 is being used)

function arena = map(filename)

%Vectors from center of bot to IR sensors

Bot_IR1 = [4.5,3.5];

Bot_IR2 = [7.5,0];

Bot_IR3 = [4.5,-3.5];

Bot_IR4 = [-4,-3.5];

Bot_IR5 = [-4,3.5];

Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5];

%Unit vectors with the same angle that the IR sensors are tilted at

Angle_IR1 = [.78,.63];

Angle_IR2 = [1,0];

Angle_IR3 = [.78,-.63];

Angle_IR4 = [0,-1];

Angle_IR5 = [0,1];

Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5];

%Define sensor model.

%Definitions:

sensor_model_width = 60;

sensor_model_height = 100;

sensor_bounds_x = 30;

sensor_bounds_y = 100;

sensor_width = 8; %cm

empty_weight = 0.1;

wall_weight = 2.0;

outside_weight = 1;

%Space creation:

Sx = linspace(-sensor_bounds_x, sensor_bounds_x, sensor_model_width);

Sy = linspace(-sensor_bounds_y, sensor_bounds_y, sensor_model_height);

sensor_model = zeros(size(Sx,1), size(Sy,1), 71);

%k ranges over distances (cm)

for k=10:80

	for j = 1:sensor_model_height;

		for i = 1:sensor_model_width;

			slope = k / sensor_width;	

			if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k - 2) && Sy(j) > 0

				sensor_model(i,j,k) = empty_weight;

			elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0

				sensor_model(i,j,k) = wall_weight / abs((k - Sy(j)));	

			else

				sensor_model(i,j,k) = outside_weight;

			end

		end

	end

end

%Define the sensor model for when no measurement is made (-1)

for j = 1:sensor_model_height;

	for i = 1:sensor_model_width;

		slope = 80 / sensor_width;	

		if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0

			sensor_model(i,j,k) = empty_weight;

		else

			sensor_model(i,j,k) = outside_weight;

		end

	end

end

%Define map area (1100cm by 1100cm)

map_width = 600;

map_height = 600;

udata = [-1, 1] * sensor_model_width/2;

vdata = [-1, 1] * sensor_model_height/2;

xdata = [-1, 1] * map_width/2;

ydata = [-1, 1] * map_height/2;

arena = ones(map_width,map_height);

%For now, read input in from a file

Data = dlmread(filename, ' ');

points = [];

%loop through each line of file

for i=1:size(Data, 1)

	display(i);

    row = Data(i, :);

    x = row(1);

    y = row(2);

    theta = row(3);

	%look at each IR sensor reading

    for ir=1:5

        distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm

        if distance ~= -1

			k_val = floor(distance / 10);

			sensor_k = sensor_model(:,:,k_val);

			%Find vector of object relative to center of bot

        	ObjectVector = distance * Angle_IR(ir, :);

        	ObjectVector = ObjectVector + Bot_IR(ir, :);

			%rotate by theta

        	objTheta = atan2(ObjectVector(2), ObjectVector(1));

        	objTheta = objTheta + theta;

		else

			sensor_k = sensor_model(:,:,71);

			%Use the maximum length to clear an empty area.

        	ObjectVector = 80 * Angle_IR(ir, :);

        	ObjectVector = ObjectVector + Bot_IR(ir, :);

			%rotate by theta

        	objTheta = atan2(ObjectVector(2), ObjectVector(1));

        	objTheta = objTheta + theta;

		end

		dx = x + Bot_IR(ir,1);

		dy = y + Bot_IR(ir,2);

		translation_mat = [1, 0, 0;

						   0, 1, 0;

						  dx,dy, 1];

		rotation_mat = [cos(objTheta), sin(objTheta), 0;

					   -sin(objTheta), cos(objTheta), 0

						   		    0,			   0, 1];

		transformation_mat = rotation_mat * translation_mat;

		translation = maketform('affine', transformation_mat);

		sensor_k = imtransform(sensor_k, translation,... 

			'UData', udata, 'Vdata', vdata,...

			'XData', xdata, 'YData', ydata,...

			'Size', [map_width, map_height], 'FillValues', 1);

		arena = arena.*sensor_k;

    end

end

image(arena.*20)