Revision 907
Code that converts data from the robot into a plot
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