clear all;

clc;

% Matrices de transformaci�n homog�nea para cada artiuclaci�n

A00=denavit(0,0.4,0,0); % eslabon 1 movil
A01=denavit(pi+pi,0.5,-0.5,pi/2); % eslabon 1 movil
A12=denavit((-3*pi)/4,0,-1,0); % eslabon 2 movil
A23=denavit(3*pi/2+pi/2+pi/4,0,0,pi/2);
A34=denavit(pi,1,0,pi/2); % eslabon 3 movil
A45=denavit(pi+pi/2,0,0,pi/2); % eslabon 4 movil
A56=denavit(pi/2,0.3,0,0); % eslabon 5 movil
% (EFECTOR FINAL)
% Ag1=denavit(0,0,-0.3,0); % griper
% Ag2=denavit(0,0.8,0,0); % griper
% Ag3=denavit(0,0,0.3,0); % griper
% Ag4=denavit(0,0.8,0,0); % griper

S00=eye(4); %matriz identidad 4x4

S0=S00*A00; % posici�n y orientaci�n (respect. al fijo xyz)
S1=A00*A01; % posici�n y orientaci�n (respct. al movil 1)
S2=A00*A01*A12; % posici�n y orientaci�n (respect. al movil 2)
S3=A00*A01*A12*A23; % posici�n y orientaci�n (efector final)
S4=A00*A01*A12*A23*A34;
S5=A00*A01*A12*A23*A34*A45;
S6=A00*A01*A12*A23*A34*A45*A56;

% S7=S4*Ag1; % PARA EL GRIPPER

% S8=S5*Ag2; % PARA EL GRIPPER
% S9=S4*Ag3; % PARA EL GRIPPER
% S10=S7*Ag4; % PARA EL GRIPPER

%Extrayendo datos de eslabones para el dibujo ("ploteo")

P00=[S00(1,4) S00(2,4) S00(3,4)];
P0=[S0(1,4) S0(2,4) S0(3,4)];
P1=[S1(1,4) S1(2,4) S1(3,4)];
P2=[S2(1,4) S2(2,4) S2(3,4)];
P3=[S3(1,4) S3(2,4) S3(3,4)];
P4=[S4(1,4) S4(2,4) S4(3,4)];
P5=[S5(1,4) S5(2,4) S5(3,4)];
P6=[S6(1,4) S6(2,4) S6(3,4)];
% P8=[S8(1,4) S8(2,4) S8(3,4)];
% P9=[S9(1,4) S9(2,4) S9(3,4)];
% P10=[S10(1,4) S10(2,4) S10(3,4)];

%-----------------------------DIBUJO------------------------------------
figure(1) %Permite crear una figura
hold on %mantiene los gr�ficos en los ejes actuales para que los nuevos
%trazados a�adidos a los ejes no eliminen los trazados existentes

%Dibujo los eslabones: P0-P1, P1-02, P2-P3... Coordenada con coordenada

plot3([P00(1) P0(1)],[P00(2) P0(2)],[P00(3) P0(3)],'c','Linewidth',1)
plot3([P0(1) P1(1)],[P0(2) P1(2)],[P0(3) P1(3)],'c','Linewidth',1)
plot3([P1(1) P2(1)],[P1(2) P2(2)],[P1(3) P2(3)],'r','Linewidth',1)
plot3([P2(1) P3(1)],[P2(2) P3(2)],[P2(3) P3(3)],'g','Linewidth',1)
plot3([P3(1) P4(1)],[P3(2) P4(2)],[P3(3) P4(3)],'m','Linewidth',1)
plot3([P4(1) P5(1)],[P4(2) P5(2)],[P4(3) P5(3)],'c','Linewidth',1)
plot3([P5(1) P6(1)],[P5(2) P6(2)],[P5(3) P6(3)],'r','Linewidth',1)
% plot3([P6(1) P7(1)],[P6(2) P7(2)],[P6(3) P7(3)],'g','Linewidth',1)
% plot3([P7(1) P8(1)],[P7(2) P8(2)],[P7(3) P8(3)],'m','Linewidth',1)
% plot3([P8(1) P9(1)],[P8(2) P9(2)],[P8(3) P9(3)],'k','Linewidth',2)
% plot3([P9(1) P10(1)],[P9(2) P10(2)],[P9(3) P10(3)],'k','Linewidth',2)

%Dibujo de sistemas de referencia

dibujar_sistema_referencia_MTH(S00, 0.2, 2, '00')
dibujar_sistema_referencia_MTH(S0, 0.2, 2, '0')
dibujar_sistema_referencia_MTH(S1, 0.2, 2, '1')
dibujar_sistema_referencia_MTH(S2, 0.2, 2, '2')
%dibujar_sistema_referencia_MTH(S3, 0.3, 1, '3')
dibujar_sistema_referencia_MTH(S4, 0.2, 2, '4')
%dibujar_sistema_referencia_MTH(S5, 0.3, 1, '5')
dibujar_sistema_referencia_MTH(S6, 0.2, 2, '6')
% dibujar_sistema_referencia_MTH(S7, 0.2, 3, '7')

grid on %Muestra la rejilla

xlabel('X'); %Nombre del eje X
ylabel('Y'); %Nombre del eje Y
zlabel('Z'); %Nombre del eje Z
title ('SIMULACION ROBOT DE X GDL'); %Titulo de la grafica
axis([-4 4 -4 4 -4 4]) %Valores de los ejes:
%X va desde -5 hasta 5
%Y va desde -5 hasta 5
%Z va desde 0 hasta 5
view(45,40) %Me permite ver el dibujo 3D desde esas coordenadas.