%%Programa para conocer la posicin y orientacin de la herramienta 1

%%(tool1) del fanuc LR Mate 200ic

%%En thetha2 colocar el valor del joint 2 -90 y en theta 3 colocar el valor del
joint 2 + joint 3
syms theta1 theta2 theta3 theta4 theta5 theta6 alpha1 alpha2 alpha3 alpha4 alpha
5 alpha6 b1 b2 b3 b4 b5 b6 a1 a2 a3 a4 a5 a6
%syms l2 d3 l6
%theta1=deg2rad(0);
%theta2=deg2rad(0-90);
%theta3=deg2rad(0);
%theta4=deg2rad(0);
%theta5=deg2rad(0);
%theta6=deg2rad(0);
alpha1=deg2rad(-90);
alpha2=deg2rad(180);
alpha3=deg2rad(-90);
alpha4=deg2rad(90);
alpha5=deg2rad(-90);
alpha6=deg2rad(180);
b1=0;
b2=0;
b3=0;
b4=-1250;
b5=0;
b6=-260;
a1=270;
a2=1050;
a3=200;
a4=0;
a5=0;
a6=0;
%if(alpha1==90||alpha1==-90)
%
cos(alpha1);
% %end
% %if(alpha2==180||alpha1==-180)
%
sin(alpha2);
% %end
% %if(alpha3==90||alpha3==-90)
%
cos(alpha3);
% %end
% %if(alpha4==90||alpha4==-90)
%
cos(alpha4);
% %end
% %if(alpha5==90||alpha5==-90)
%
cos(alpha5);
% %end
% %if(alpha6==180||alpha6==-180)
%
sin(alpha6);
% %end

for i=0:1:6
switch(i)
case 1

A1=[cos(theta1) -sin(theta1)*round(cos(alpha1)) sin(theta1)*sin(alpha1) a1*cos(t

heta1);
sin(theta1) cos(theta1)*round(cos(alpha1)) -cos(theta1)*sin(alpha1) a1*sin(theta
1);
0 sin(alpha1) round(cos(alpha1)) b1;
0 0 0 1]
%pause;
case 2
A2=[cos(theta2) -sin(theta2)*cos(alpha2) sin(theta2)*round(sin(alpha2)) a2*cos(t
heta2);
sin(theta2) cos(theta2)*cos(alpha2) -cos(theta2)*round(sin(alpha2)) a2*sin(theta
2);
0 round(sin(alpha2)) cos(alpha2) b2;
0 0 0 1]
%pause;
case 3
A3=[cos(theta3) -sin(theta3)*round(cos(alpha3)) sin(theta3)*sin(alpha3) a3*cos(t
heta3);
sin(theta3) cos(theta3)*round(cos(alpha3)) -cos(theta3)*sin(alpha3) a3*sin(theta
3);
0 sin(alpha3) round(cos(alpha3)) b3;
0 0 0 1]
%pause;
case 4
A4=[cos(theta4) -sin(theta4)*round(cos(alpha4)) sin(theta4)*sin(alpha4) a4*cos(t
heta4);
sin(theta4) cos(theta4)*round(cos(alpha4)) -cos(theta4)*sin(alpha4) a4*sin(theta
4);
0 sin(alpha4) round(cos(alpha4)) b4;
0 0 0 1]
%pause;
case 5
A5=[cos(theta5) -sin(theta5)*round(cos(alpha5)) sin(theta5)*sin(alpha5) a5*cos(t
heta5);
sin(theta5) cos(theta5)*round(cos(alpha5)) -cos(theta5)*sin(alpha5) a5*sin(theta
5);
0 sin(alpha5) round(cos(alpha5)) b5;
0 0 0 1]
case 6
A6=[cos(theta6) -sin(theta6)*cos(alpha6) sin(theta6)*round(sin(alpha6)) a6*cos(t
heta6);
sin(theta6) cos(theta6)*cos(alpha6) -cos(theta6)*round(sin(alpha6)) a6*sin(theta
6);
0 round(sin(alpha6)) cos(alpha6) b6;
0 0 0 1]
T=A1*A2*A3*A4*A5*A6;
end
end