%CINEMATICA DE MAQUINARIA - LAB 2 - FRANCISCO FERNANDEZ // 12/11/2018

% METODOS NUMERICOS APLICADOS A CINEMATICA DE MAQ

%DATOS
r1=5.5;
r2=1; r3=6; r4=2.5;
N=40;
tetha2=[0:2*pi/N:2*pi];%VALORES INICIALES ARBITRARIOS
t=[22*pi/180;65*pi/180];
syms t4 t3
for i=1:N+1
fn=[r1+r4*cos(t4)-r2*cos(tetha2(i))-r3*cos(t3);r4*sin(t4)-r2*sin(tetha2(i))-
r3*sin(t3)]; %SISTEMA NO LINEAL%
fp=jacobian(fn); %JACOBIANO DEL SISTEMA
tol=0.01;
f=inline(fn); %TRANSFORMANDO F PARA EVALUAR
jf=inline(fp); %TRANSFORMANDO EL JACOBIANO PARA EVALUAR
error=norm(f(t(1),t(2)),2); %SACANDO UN VALOR INICIAL PARA COMPARACION
while error>=tol
ft=f(t(1),t(2));
fpt=jf(t(1),t(2));
t1=t-inv(fpt)*ft;%METODO DE NEWTON
error=norm(f(t1(1),t1(2)),2); %VERIFICACION DE APROXIMACION A CERO
t=t1;
end
tetha4(i)=t(1);
tetha3(i)=t(2);
end