classdef robot_RRRR<handle

%ROBOT PLANAR

properties
%PARAMETROS MECANICOS
LL1
LL2
LL3
LL4
%PARAMETROS D.H
d1
a1
alpha1
d2
a2
alpha2
d3
a3
alpha3
d4
a4
alpha4
end

methods
% CONSTRUCTOR
function this= robot_RRRR()
%Inicializamos parametros
this.a1=0; %fijo
this.alpha1=pi/2; %fijo
this.d2=0;
this.alpha2=0;
this.d3=0;
this.alpha3=0;
this.d4=0;
this.alpha4=0;
end
function set_dimensions(this,L1,L2,L3,L4)
%Metodo para asignar las dimensiones
this.LL1=L1;
this.LL2=L2;
this.LL3=L3;
this.LL4=L4;
%Ponemos Los parametros DH q faltan
this.d1=L1;
this.a2=L2;
this.a3=L3;
this.a4=L4;

end
function q=cinematica_inversa(this,xc,yc,zc)
%Metodo para la cinematica inversa
L1=this.LL1;
L2=this.LL2;
 L3=this.LL3;
L4=this.LL4;

%%%%%%%%%%%%%%%%
%Angulo "theta 1"
q1_star=atan2(yc,xc);
%Angulo "theta3"
r2=xc^2+yc^2;
r1=sqrt(r2);
r=r1-L4;
s=zc-L1;
num=r^2+s^2-L2^2-L3^2;
den=2*L2*L3;
D=num/den;
if(abs(D)>1)
error('POSICION IMPOSIBLE DE ALCANZAR')
end
q3_star=atan2(+sqrt(1-D^2),D);
q2_star=atan2(s,r)-...
atan2(L3*sin(q3_star),L2+L3*cos(q3_star));
q4_star=-1*(q2_star + q3_star);
%Vector de ANgulos
q=[q1_star q2_star q3_star q4_star];
end
function A1=compute_matrix_A1(this,theta1)
%Metodo para hallar A1
A1=matriz_homogenea_DH(theta1,...
this.d1,this.a1,this.alpha1);
end

function A2=compute_matrix_A2(this,theta2)
%Metodo para hallar A2
A2=matriz_homogenea_DH(theta2,...
this.d2,this.a2,this.alpha2);
end

function A3=compute_matrix_A3(this,theta3)
%Metodo para hallar A3
A3=matriz_homogenea_DH(theta3,...
this.d3,this.a3,this.alpha3);
end
function A4=compute_matrix_A4(this,theta4)
%Metodo para hallar A3
A4=matriz_homogenea_DH(theta4,...
this.d4,this.a4,this.alpha4);
end
end
end