Revision 907
Code that converts data from the robot into a plot
trunk/code/projects/mapping/matlab/map.m  

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,:), '*'); 
trunk/code/projects/mapping/matlab/input.txt  

1 
17 0 0 1 42 1 1 1 

2 
18 0 0 1 42 1 1 1 

3 
19 0 0 1 42 1 1 1 

4 
20 0 0 1 42 1 1 1 
Also available in: Unified diff