Revision 907

View differences:

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