%% Transf. de ref. Atlas20xx
%%
close all; clear all;clc;

%camera 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%    INPUTS   %%%%

%distancias
D1_0=170;
D2_0=134;
D3_0=634;
L4_0=17;
L6_0=31;

%angulos
Q4_0=32;
Q5_0=20; 
Q6_0=0;

Q4_0=Q4_0*pi/180;
Q5_0=Q5_0*pi/180;
Q6_0=Q6_0*pi/180;

% Matriz Denavit-Hartenberg (teta,l,d,alpha)
   
DH=[pi/2 0 0 pi/2
    pi/2 0 D1_0 pi/2
    pi/2 0 D2_0 pi/2
    0 0 D3_0 -pi/2
    Q4_0 L4_0 0 pi/2
    pi/2+Q5_0 0 0 pi/2  
    Q6_0 0 L6_0 0];
AA=Tlinks(DH);
Org=LinkOrigins(AA);
h=DrawLinks(Org);
set(h, 'LineWidth',2,'Marker','Diamond')
hold on;grid on; axis square;axis equal
run seixos
DrawFrames(AA,P,'k');
T_2=eye(4);
for n=1:size(AA,3)
    T_2=T_2*AA(:,:,n);
end
xlabel('X')
ylabel('Y')
zlabel('Z')

%camera 1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%    INPUTS   %%%%

%distancias
D1_1=D1_0;
D2_1=-D2_0;
D3_1=D3_0;
L4_1=L4_0;
L6_1=L6_0;

%ângulos
Q4_1=32;
Q5_1=-26; 
Q6_1=0;
Q4_1=Q4_1*pi/180;
Q5_1=Q5_1*pi/180;
Q6_1=Q6_1*pi/180;

% Matriz Denavit-Hartenberg (teta,l,d,alpha)

   DH=[pi/2 0 0 pi/2
    pi/2 0 D1_1 pi/2
    pi/2 0 D2_1 pi/2
    0 0 D3_1 -pi/2
    Q4_1 L4_1 0 pi/2
    pi/2+Q5_1 0 0 pi/2  
   Q6_1 0 L6_1 0];

AA=Tlinks(DH);
Org=LinkOrigins(AA);
h=DrawLinks(Org);
set(h, 'LineWidth',2,'Marker','Diamond')
DrawFrames(AA,P,'k');

T_1=eye(4);
for n=1:size(AA,3)
    T_1=T_1*AA(:,:,n);
end
axis([-10 300 -200 200 -5 660])
view(115,28)