## Revision 907

Code that converts data from the robot into a plot

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