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