root / branches / analog / code / projects / mapping / matlab / map.m @ 1390
History | View | Annotate | Download (2.16 KB)
1 | 907 | erhart | %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,:), '*'); |