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


2 


3 
%Input comes from file where each line contains 8 spaceseparated values


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


5 
%Example line of input file:


6 
%17 23 .45 1 42 1 1 1


7 
%x and y are centimeters, theta is in radians


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


9 


10 
%Output: graph of points seen by robot


11 
%All output measurements are in centimeters


12 


13 
%TODO


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


15 
function map()


16 


17 
%Vectors from center of bot to IR sensors


18 
Bot_IR1 = [4.5,3.5];


19 
Bot_IR2 = [7.5,0];


20 
Bot_IR3 = [4.5,3.5];


21 
Bot_IR4 = [4,3.5];


22 
Bot_IR5 = [4,3.5];


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


24 


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


26 
Angle_IR1 = [.78,.63];


27 
Angle_IR2 = [1,0];


28 
Angle_IR3 = [.78,.63];


29 
Angle_IR4 = [0,1];


30 
Angle_IR5 = [0,1];


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


32 


33 
%For now, read input in from a file


34 
Data = dlmread('input.txt', ' ');


35 


36 
points = [];


37 
%loop through each line of file


38 
for i=1:size(Data, 1)


39 
row = Data(i, :);


40 
x = row(1);


41 
y = row(2);


42 
theta = row(3);


43 
%look at each IR sensor reading


44 
for ir=1:5


45 
if row(ir+3) ~= 1


46 
%Find vector of object relative to center of bot


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


48 
ObjectVector = distance * Angle_IR(ir, :);


49 
ObjectVector = ObjectVector + Bot_IR(ir, :);


50 


51 
%rotate by theta


52 
objR = sqrt(ObjectVector(1)^2 + ObjectVector(2)^2);


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


54 
objTheta = objTheta + theta;


55 


56 
%add absolute position


57 
objX = (objR * cos(objTheta)) + x;


58 
objY = (objR * sin(objTheta)) + y;


59 
ObjectVector = [objX, objY];


60 


61 
points = [points; ObjectVector];


62 
end


63 
end


64 
end


65 


66 
%graph all the points


67 
figure;


68 
toGraph = points';


69 
plot(toGraph(1,:), toGraph(2,:), '*');
