MECATRONICA

FUNDAMENTOS DE ROBOTICA

JAVIER DE LA PEA TREJO

UBICACIN ESPACIAL

8 CUATRIMESTRE MEC03SV-15

LIRA GONZALEZ JUAN PABLO

Enero de 2016
 PROGRAMA

%-------------------VARIABLES DE CONTROL----------------------------
clearvars;clc;n=0;b=0;continuar=0;o=1;r=1;ii=1;
%-----------------DATOS INICIALES DEL SISTEMA ---------------------
sistema = input('CUAL ES EL TIPO DE SISTEMA?','s');
x1=input('TECLEA LOS VALORES INICIALES DEL SISTEMA \n TECLEA EL VALOR DE -X- : ');
y1=input('TECLEA LOS VALORES DE -Y- : ');
z1=input('TECLEA LOS VALORES DE -Z- : ');
%-----------------VALORES PARA GRAFICAR-----------------------------
AX(o)=x1;AY(o)=y1;AZ(o)=z1; x2=x1; y2=y1; z2=z1;
C=[x2;y2;z2;1];
%-----------------CICLO PARA INTRODUCIR DATOS----------------------
while continuar == 0
seleccion=input('INTRODUCE LOS CAMBIOS QUE HIZO EL SISTEMA (ROTACION O
TRASLACION) : ','s');
switch seleccion
case 'ROTACION'
n=n+1; o=o+1;
rotar=input('SOBRE QUE EJE ESTA ROTANDO : ','s');
angulo=input('CUAL ES EL VALOR DEL ANGULO : ');
if rotar == 'X' | rotar == 'U'
TR=[1,0,0,0;0,cosd(angulo),-
sind(angulo),0;0,sind(angulo),cosd(angulo),0;0,0,0,1];
else
if rotar == 'Y' | rotar == 'V'
TR=[cosd(angulo),0,sind(angulo),0;0,1,0,0;-
sind(angulo),0,cosd(angulo),0;0,0,0,1];
else
if rotar == 'Z' | rotar == 'W'
TR=[cosd(angulo),-
sind(angulo),0,0;sind(angulo),cosd(angulo),0,0;0,0,1,0;0,0,0,1];
else
disp('DATOS INCORRECTOS')
end
end
end
H(:,:,n)= TR;
titulo=strcat('\n','T(',rotar,',',int2str(angulo),')','\n');
fprintf(titulo);
disp(TR);
case 'TRASLACION'
n=n+1; o=o+1;
x=input('TECLEA EL TRASLADO EN -X- : ');
y=input('TECLEA EL TRASLADO EN -Y- : ');
z=input('TECLEA EL TRASLADO EN -Z- : ');
TT=[1,0,0,x;0,1,0,y;0,0,1,z;0,0,0,1];
H(:,:,n)= TT;

titulo=strcat('\n','T(',int2str(x),',',int2str(y),',',int2str(z),')','\n');fprintf(
titulo);
disp(TT);
otherwise
disp('OPCION INCORRECTA')
end
s=input('SI YA ACABASTE TECLEA -S- : ','s');
if(s == 'S')
 continuar =1;
else
continuar= 0;
end
end
%-----------------POS-MULTIPLICACION Y %PRE-MULTIPLICACION----------------------
switch sistema
case 'FIJO'

case 'MOVIL'
end
if n ~= 1
switch sistema
case 'FIJO'
%GRAFICANDO
while r ~= n+1
U=H(:,:,r)*C;
AX(r+1)=U(1,1);AY(r+1)=U(2,1);AZ(r+1)=U(3,1);
C=[U(1,1);U(2,1);U(3,1);1];
r=r+1;
end
k=n; l=1; g=n;
case 'MOVIL'
r=n
while r ~= 0
U=H(:,:,r)*C;
AX(ii+1)=U(1,1);AY(ii+1)=U(2,1);AZ(ii+1)=U(3,1);
C=[U(1,1);U(2,1);U(3,1);1];

r=r-1;
ii=ii+1;
end
k=1; l=n; g=1;
end
while k ~= l
switch sistema
case 'FIJO'
k=k-1;
case 'MOVIL'
k=k+1;
end
if b == 0
T = H(:,:,g)*H(:,:,k);
b=1;
else
T = T*H(:,:,k);
end
end
else
T=H(:,:,n)
%GRAFICAR SOLO SI SE HACE UN CAMBIO
U=H(:,:,n)*C;
AX(o)=U(1,1);AY(o)=U(2,1);AZ(o)=U(3,1);
end
%---------------------IMPRIMIR RESULTADOS--------------------------------
clc
fprintf('\n MATRIZ DE TRANSFORMACION HOMOGENEA DEL SISTEMA : \n');
disp(T)
fprintf('\n COORDENAS DEL VERCTOR RESULTANTE : \n');
disp(T*[x1;y1;z1;1])
plot3(AX,AY,AZ,'bo-','markersize',6,'markerfacecolor','r'), grid; %GRAFICANDO
xlabel('EJE X');
ylabel('EJE Y');
zlabel('EJE Z');
titulo2=strcat('TIPO DE SISTEMA -',sistema);
title(titulo2);
PItext=strcat(' PI(',int2str(x1),',',int2str(y1),',',int2str(z1),')');
PFtext=strcat(' PF(',int2str(U(1,1)),',',int2str(U(2,1)),',',int2str(U(3,1)),')');
text(x1,y1,z1,PItext);
text(U(1,1),U(2,1),U(3,1),PFtext);