root / trunk / code / projects / mapping / matlab / map.m @ 907
History  View  Annotate  Download (2.16 KB)
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,:), '*'); 