%MECANISMOS-UNSAAC

%posicion mecanismo BIELA MANIVELA DESCENTRADA

% CCARITA QQUESHUALLPA,Nicanor =====>carita Jr.
%codigo=124154
clc,clear
clear
AM=[0,0,0,0,0];
chi=0;
a=0.15; %longitud del eslabon 2
b=0.45; %longitud del eslabon 3
c=0.3;% longitud descentrada variable arbitrario
t2=50*pi/180; % inicio de la rotacion del eslabon 2=b
t3=300.*pi/180;% angulo arbitrario de t3
cr=-1.5;

for i=1:5
f1=a.*cos(t2)+b.*cos(t3)-c;
f2=a.*sin(t2)+b.*sin(t3);
A=[-a.*sin(t2) -b.*sin(t3);a.*cos(t2) b.*cos(t3)]; %EL JACOBIANO
B=-[a.*cos(t2)+b.*cos(t3)-c;a.*sin(t2)+b.*sin(t3)];
X=inv(A)*B;
vt2=X(1);%variacion de tetha2
vt3=X(2);%variacion de tetha 3

e=0.000001; %error asumido

while abs(vt2)>e && abs(vt3)>e

t2=t2+vt2;
t3=t3+vt3;
f1=a.*cos(t2)+b.*cos(t3)-c;
f2=a.*sin(t2)+b.*sin(t3);
A=[-a.*sin(t2) -b.*sin(t3);a.*cos(t2) b.*cos(t3)]; %EL JACOBIANO
B=-[a.*cos(t2)+b.*cos(t3)-c;a.*sin(t2)+b.*sin(t3)];
X=inv(A)*B;
vt2=X(1);%variacion de tetha2
vt3=X(2);%variacion de tetha 3

end

%velocidades
A=[-a.*sin(t2) -b.*sin(t3);a.*cos(t2) b.*cos(t3)]; %EL JACOBIANO
C=[cr;0];
Y=inv(A)*C;
w2=Y(1);
w3=Y(2);

M1=[c,t2*180/pi,t3*180/pi,w2,w3];

if chi==0
AM=M1;
chi=1;
else
AM=[AM;M1];
end
 c=c+0.01;

end

disp (' c t2 t3 w2 w3 ')

disp (AM)
c=AM(:,1);
t2=AM(:,2);
t3=AM(:,3);
w2=AM(:,4);
w3=AM(:,5);

figure (1)
plot(c,t2)
title('t2 en funciuon de c')
xlabel('c')
ylabel('t2')
grid
figure (2)
plot(c,w2)
title('w2 en funcion de c')
xlabel('c')
ylabel('w2')
grid