Statistics
| Revision:

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 space-separated 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,:), '*');