| blob | c6d1ecd2641323d580193b70c17fd2a925f9e3c0 |
1 classdef Transform <handle
2 %TRANSFORM Modul pro prevod mezi souradnicemi v rovine obrazovky a uhlz
3 %jednotlivych motoru
5 properties (Constant)
6 ElevHeight = -62; %a vyska elevacni osy
7 ElCamAxisDist = 62; %c vertikalni vzdalenost mezi elevacni a kamerovou osou
8 AzElDist = 31; % b bocni vzdalenost mezi osami
9 FocusOffset = 22; %d pozice ohniska kamery ve smeru "vpred vzad"
10 DisplayPosition = 540; %vzdalenost displeje od podstavce kamery, muze byt libovolne, jen pro vypocty
11 Inc2Rad = -0.654*pi/180; %konstanta pro prevod z jednotek senzoru na radianyo
12 Pic2RadX = -4.5e-4;%-4.5e-4; %konstanta pro prevod z pixelu kamery na odchylkovy uhel
13 Pic2RadY = -4.4e-4; %konstanta pro prevod z pixelu kamery na odchylkovy uhel
14 xDisplayOffset = 0;%158; %posun pocatku souradnic displeje v ose X
15 yDisplayOffset = 0;%150; %posun pocatku souradnic displeje v ose Y
16 end
18 properties (SetAccess = private)
19 %model pro zamerovani cilove pozice
20 aimRobot;
21 %model pro urceni pozice komara v prostoru
22 camRobot;
23 end
25 methods
26 %vytvori objekt pro transformaci souradnic
27 function obj = Transform()
28 L1 = link([pi/2,0,0,obj.ElevHeight]);
29 L2 = link([0,obj.ElCamAxisDist,0,-obj.AzElDist]);
30 L6 = link([pi/2,obj.ElCamAxisDist,0,-obj.AzElDist]);
31 L3 = link([pi/2,0,0,0]);
32 L4 = link([pi/2,0,0,0]);
33 L5 = link([0,0,pi,0, 1]);
34 obj.aimRobot = robot({L1,L6,L5,L4,L4,L4});
35 obj.camRobot = robot({L1,L2,L3,L4,L5});
36 end
38 %prevede kartezske souradnice na hodnoty pro nastaveni kamery
39 %vystupni hodnoty jsou primo pulsy pro nastaveni motoru
40 function [az,el] = kart2robFast(obj, x, y)
41 %q = ikine(obj.aimRobot, [eye(3),[obj.DisplayPosition;-x+obj.xDisplayOffset;-y+obj.yDisplayOffset];0,0,0,1],[0,pi/2,obj.DisplayPosition,0,0,0]);
42 alpha1 = atan2((x + obj.xDisplayOffset + obj.AzElDist),obj.DisplayPosition);
43 dx = obj.AzElDist * cos(alpha1);
44 dz = obj.AzElDist * sin(alpha1);
45 alpha = -atan2((x + obj.xDisplayOffset + dx),(obj.DisplayPosition - dz));
46 tmp = mod(alpha,2*pi);
47 if(tmp > pi)
48 tmp = tmp - 2 * pi ;
49 end
50 az= (tmp/obj.Inc2Rad) + 127;
52 L = sqrt((x + obj.xDisplayOffset + dx)^2 + (obj.DisplayPosition - dz)^2);
53 beta1 = atan2((y + obj.yDisplayOffset + obj.ElevHeight + obj.ElCamAxisDist),L);
54 dy = obj.ElCamAxisDist * cos(beta1);
55 dz2 = obj.ElCamAxisDist * sin(beta1);
56 beta = -atan2((y + obj.yDisplayOffset + obj.ElevHeight + dy),(L - dz2));
57 tmp = mod(beta - 2* pi,2*pi);
58 if(tmp > pi)
59 tmp = tmp - 2 * pi;
60 end
61 el= (tmp/obj.Inc2Rad) + 127;
62 end
64 %prevede kartezske souradnice na hodnoty pro nastaveni kamery
65 %vystupni hodnoty jsou primo pulsy pro nastaveni motoru
66 function [az,el] = kart2rob(obj, x, y)
67 q = ikine(obj.aimRobot, [eye(3),[obj.DisplayPosition;-x+obj.xDisplayOffset;-y+obj.yDisplayOffset];0,0,0,1],[0,pi/2,obj.DisplayPosition,0,0,0]);
68 tmp = mod(q(1),2*pi);
69 if(tmp > pi)
70 tmp = tmp - 2 * pi ;
71 end
72 az= (tmp/obj.Inc2Rad) + 127;
73 tmp = mod(q(2) - pi/2,2*pi);
74 if(tmp > pi)
75 tmp = tmp - 2 * pi;
76 end
77 el= (tmp/obj.Inc2Rad) + 127;
78 end
80 %prevede natoceni uhel natoceni kamery na kartezske souradnice v
81 %rovine obrazovky
82 %vstupem je uhel udany v hodnotach, se kterymi pracuji motory
83 function [x,y] = rob2kart(obj, az, el)
84 [x,y] = pict2kart(obj,az,el,0,0);
85 end
87 %Zjisti polohu objektu z danych souradnic v obraze kamery na
88 %souradnice v rovine obrazovky
89 %vstupem je uhel udany v hodnotach, se kterymi pracuji motory
90 %souradnice jsou pocitany od strdoveho krize
91 function [x,y] = pict2kart(obj, az, el, px, py)
92 T = fkine(obj.camRobot, [(az - 127)*obj.Inc2Rad ,(el - 127)*obj.Inc2Rad + pi/2,(py*obj.Pic2RadY) + pi/2,(px*obj.Pic2RadX) - pi/2,0]);
93 d = (obj.DisplayPosition - T(1,4))/T(1,3);
94 x = (-d * T(2,3) - T(2,4)) + obj.xDisplayOffset;
95 y = (-d * T(3,3) - T(3,4)) + obj.yDisplayOffset;
96 end
98 function [az,el] = pict2robAAA(obj, az, el, px, py)
99 posunMax = 4;
100 osaX = posunMax/320;
101 osaY = posunMax/240;
103 difAz = 0;
104 difEl = 0;
105 if px < 320
106 difAz = -((320-px)*osaX);
107 end
108 if px > 320
109 difAz = ((px-320)*osaX);
110 end
111 if py < 240
112 difEl = -((240-py)*osaY);
113 end
114 if py > 240
115 difEl = +((py-240)*osaY);
116 end
118 az = az + difAz;
119 el = el + difEl;
120 end
123 %zobrazi model kamery pro zamereni bodu v prostoru v dane pozici
124 %uhly jsou zadavany v radianech
125 function plotAimBotAng(obj, az, el, len)
126 plot(obj.aimRobot, [ az , el + pi/2,len, 0, 0, 0]);
127 end
129 %zobrazi model kamery pro urceni polohy bjektu z obrazu v dane
130 %pozici
131 %uhly jsou zadavany v radianech
132 function plotCamBotAng(obj, az, el, len, dx, dy)
133 plot(obj.camRobot, [az ,el + pi/2,dx*obj.Pic2RadX + pi/2,dy*obj.Pic2RadY - pi/2,len]);
134 end
136 %zobrazi model kamery pro zamereni bodu v prostoru v dane pozici
137 %uhly jsou zadavany v hodnotach se kterymi pracuji motory
138 function plotAimBotInc(obj, az, el, len)
139 obj.plotAimBotAng((az - 127)*obj.Inc2Rad ,(el - 127)*obj.Inc2Rad , len);
140 end
142 %zobrazi model kamery pro urceni polohy bjektu z obrazu v dane
143 %pozici
144 %uhly jsou zadavany v hodnotach se kterymi pracuji motory
145 function plotCamBotInc(obj,az, el, len, dx, dy)
146 obj.plotCamBot((az - 127)*obj.Inc2Rad,(el - 127)*obj.Inc2Rad,dx,dy,len);
147 end
150 end
152 end