cinematica.m

Simulador de orientacion posicion de un brazo robotico de cuatro grados de libertad, en GUI de matla

%Funcion que evalua la matriz Ai. Se debe incresar en el orden
%correspondiente los siguientes valores:
%a=Angulo de giro alrededor del eje Z
%b=Desplazamiento en el eje Z
%c=Desplazamiento en el eje X
%d=Angulo de giro al rededor del eje Z

function t = cinematica(a, b, c, d)

if nargin == 4,
		
    theta = a;
    dn = b;
    an = c;
    alpha = d;
    
    if (alpha==pi | alpha==(-pi) | alpha==(2*pi))
		sa=0; ca=cos(alpha);
    else
		if (alpha==pi/2 | alpha==(3*pi)/2 | alpha==(-pi/2))
			ca=0; sa=sin(alpha);
		else
			sa = sin(alpha); ca = cos(alpha);		
		end
	end

    if (theta==pi | theta==(-pi) | theta==(2*pi))
		st=0; ct=cos(theta);
	else
		if (theta==pi/2 | theta==(3*pi)/2 | theta==(-pi/2))
			ct=0; st=sin(theta);
		else
			st = sin(theta); ct = cos(theta);		
		end
	end
          
   t = [	ct	-ca*st	sa*st		an*ct
			st	ca*ct	-sa*ct	an*st
			0	sa	ca		dn
			0		0		0		1];
       
 else
    error('faltan elementos')
 end