%Inicio del programa

%Solicitando los grados de libertad

disp('<<< INGRESE LOS GRADOS DE LIBERTAD DEL ROBOT >>>');
g(1)=input('\n Ingrese el grado de libertad de la base (-185 a 185): ');
%Condicionando cada grado de libertad para que se ajuste a lo posible
while g(1)>185||g(1)<-185
disp('<<< Grado de libertad imposible >>>')
g(1)=input('\n Ingrese el grado de libertad de la base (-185 a 185): ');
end
g(2)=input('\n Ingrese el grado de libertad del brazo inferior (130 a -20 ): ');
while g(2)>130||g(2)<-20
disp('<<< Grado de libertad imposible >>>')
g(2)=input('\n Ingrese el grado de libertad del brazo inferior (130 a -20 ):
');
end
g(3)=input('\n Ingrese el grado de libertad del brazo superior (-94 a 150): ');
while g(3)>150||g(3)<-94
disp('<<< Grado de libertad imposible >>>')
g(3)=input('\n Ingrese el grado de libertad del brazo superior (-94 a 150): ')
;
end
g(4)=input('\n Ingrese el grado de libertad del brazo giratorio (-350 a 350): ');
while g(4)>350||g(4)<-350
disp('<<< Grado de libertad imposible >>>')
g(4)=input('\n Ingrese el grado de libertad del brazo giratorio (-350 a 350):
');
end
g(5)=input('\n Ingrese el grado de libertad de la mueca del robot (-118 a 118): ')
;
while g(5)>119||g(5)<-118
disp('<<< Grado de libertad imposible >>>')
g(5)=input('\n Ingrese el grado de libertad de la mueca del robot (-118 a 118):
');
end
g(6)=input('\n Ingrese el grado de libertad de la punta (-350 a 350): ');
while g(6)>351||g(6)<-350
disp('<<< Grado de libertad imposible >>>')
g(6)=input('\n Ingrese el grado de libertad de la punta (-350 a 350): ');
end
%conversin de grados a radianes
g(1)=(g(1)*pi)/180;
g(2)=(g(2)*pi)/180;
g(3)=(g(3)*pi)/180;
g(4)=(g(4)*pi)/180;
g(5)=(g(5)*pi)/180;
g(6)=(g(6)*pi)/180;
%cantidad de pasos para generar la representacion del movimiento
x(1)=g(1)/30;
x(2)=g(2)/30;
x(3)=g(3)/30;
x(4)=g(4)/30;
x(5)=g(5)/30;
x(6)=g(6)/30;
for i=1:6;
for T=x(i):x(i):g(i)
x(i)=T;

O=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];

L1=3;
T1=[1 0 0 0;0 1 0 0;0 0 1 L1;0 0 0 1];
R1=[cos(x(1)) -sin(x(1)) 0 0;sin(x(1)) cos(x(1)) 0 0; 0 0 1 0;0 0 0 1];
L2=3;
T2=[1 0 0 0;0 1 0 0;0 0 1 L2;0 0 0 1];
link1=O*L1*T1*R1*T2;
L3=2;
T3=[1 0 0 -L3;0 1 0 0;0 0 1 0;0 0 0 1];
Rx1=[1 0 0 0; 0 cos(pi/2) -sin(pi/2) 0; 0 sin(pi/2) cos(pi/2) 0; 0 0 0 1
];
R2=[cos(x(2)) -sin(x(2)) 0 0;sin(x(2)) cos(x(2)) 0 0; 0 0 1 0;0 0 0 1];
link2=T3*Rx1*R2;
Rz1=[cos(pi/2) -sin(pi/2) 0 0; sin(pi/2) cos(pi/2) 0 0; 0 0 1 0; 0 0 0 1
];
L4=8;
T4=[1 0 0 L4;0 1 0 0;0 0 1 0;0 0 0 1];
R3=[cos(x(3)) -sin(x(3)) 0 0;sin(x(3)) cos(x(3)) 0 0; 0 0 1 0;0 0 0 1];
link3=Rz1*T4*R3;
Rz2=[cos(-pi/2) -sin(-pi/2) 0 0; sin(-pi/2) cos(-pi/2) 0 0; 0 0 1 0; 0
0 0 1];
L5=6;
T5=[1 0 0 L5;0 1 0 0;0 0 1 0;0 0 0 1];
Ry1=[cos(-pi/2) 0 sin(-pi/2) 0; 0 1 0 0; -sin(-pi/2) 0 cos(-pi/2) 0; 0 0
0 1];
R4=[cos(x(4)) -sin(x(4)) 0 0;sin(x(4)) cos(x(4)) 0 0; 0 0 1 0;0 0 0 1];
link4=Rz2*T5*Ry1*R4;
L6=2;
T6=[1 0 0 0;0 1 0 0;0 0 1 -L6;0 0 0 1];
Ry2=[cos(pi/2) 0 sin(pi/2) 0; 0 1 0 0; -sin(pi/2) 0 cos(pi/2) 0; 0 0 0 1
];
R5=[cos(x(5)) -sin(x(5)) 0 0;sin(x(5)) cos(x(5)) 0 0; 0 0 1 0;0 0 0 1];
link5=T6*Ry2*R5;
L7=2;
T7=[1 0 0 L7;0 1 0 0;0 0 1 0;0 0 0 1];
Ry3=[cos(-pi/2) 0 sin(-pi/2) 0; 0 1 0 0; -sin(-pi/2) 0 cos(-pi/2) 0; 0 0
0 1];
R6=[cos(x(6)) -sin(x(6)) 0 0;sin(x(6)) cos(x(6)) 0 0; 0 0 1 0;0 0 0 1];
link6=T7*Ry3*R6;
L8=1;
T8=[1 0 0 L8;0 1 0 0;0 0 1 0;0 0 0 1];
link7=T8;

%Matrices de transfrmacin
P1=O*L1*T1*R1*T2;
P2=P1*T3*Rx1*R2;
P3=P2*Rz1*T4*R3;
P4=P3*Rz2*T5*Ry1*R4;
P5=P4*T6*Ry2*R5;

P6=P5*T7*Ry3*R6;
P7=P6*T8;

v1=[0 P1(1,4) P2(1,4) P3(1,4) P4(1,4) P5(1,4) P6(1,4) P7(1,4)];

v2=[0 P1(2,4) P2(2,4) P3(2,4) P4(2,4) P5(2,4) P6(2,4) P7(2,4)];
v3=[0 P1(3,4) P2(3,4) P3(3,4) P4(3,4) P5(3,4) P6(3,4) P7(3,4)];
v4=[0 0 -2 -2 4 6 8 8];
v5=[0 0 0 0 0 0 0 -1];
v6=[0 6 6 14 14 14 14 14];
plot3(3*v4,3*v5,3*v6,'-k.')
hold on
grid on
axis square
title('KR 500-3')
xlabel('x*100')
ylabel('y*100')
zlabel('z*100')
plot3(v1,v2,v3,'-r.')
plot3(20,20,0)
plot3(40,40,0)
plot3(-30,-30,0)
plot3(0,0,80)
plot3(-40,-40,0)
hold off
pause(0.00000000001)
end
end
legend('Posicion incial','Posicion final')