%INGENIERIA MECANICA UNSAAC

%ARTURO MACEDO SILVA

%PROGRAMA CUADRICULA ARTICULADA
%POSICIONAMIENTO, VELOCIDADES ANGULARES, ACELERACIONES ANGULARES
%ejemplo planteado en la página 413 Mabie
clc,clear
clear
AM=[0,0,0,0,0,0,0,0,0,0,0,0,0,0];
chi=0;

a=3; %longitud del eslabón 2

b=8; %longitud del eslabón 3
c=6; %longitud del eslabón 4
d=7; %longitud del eslabón 1
%angulos iniciales
t1=0*pi/180;
t2=0*pi/180;%empezamos de cero para que el programa corra desde cero
t3=40*pi/180; %arbitrario considere estos ángulos lo más reales
t4=75*pi/180; %arbitrario considere estos ángulos lo más reales
w2=1;%velocidad angular del eslabón2 dato
alfa2=1;%aceleración angular del eslabón 2 dato

for k=1:73
f1=a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d*cos(t1) %funciones
f2=a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d*sin(t1) %funciones

A=[-b.*sin(t3) c.*sin(t4);
b.*cos(t3) -c.*cos(t4)]%EL JACOBIANO
B=-[a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d*cos(t1);
a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d*sin(t1)]

X=A\B
vt3=X(1)
vt4=X(2)

k=0.000001;

while abs(vt3)>k & abs(vt4)>k

t3=t3+vt3;
t4=t4+vt4;
f1=a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d*cos(t1)
f2=a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d*sin(t1)

A=[-b.*sin(t3) c.*sin(t4);
b.*cos(t3) -c.*cos(t4)]; %EL JACOBIANO
B=-[a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d*cos(t1);
a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d*sin(t1)];

X=A\B;
vt3=X(1);
vt4=X(2);
end
%--------------------------------------------------------------
%ecuaciones de velocidades
 % definimos velocidades angulares w3,w4
Jv=[-b*sin(t3) c*sin(t4);
b*cos(t3) -c*cos(t4)];
B=[a*w2*sin(t2);-a*w2*cos(t2)];
X=Jv\B;
w3=X(1);
w4=X(2);

%------------------------------------------------------
%definimos aceleraciones angulares alfa3 y alfa4
%observe que el Jacobiano se mantiene, y la matriz B considera las
%aceleraciones tangenciales y normales
Ja=[-b*sin(t3) c*sin(t4);
b*cos(t3) -c*cos(t4)];
acel=[a*alfa2*sin(t2)+a*w2^2*cos(t2)+b*w3^2*cos(t3)-c*w4^2*cos(t4);
-a*alfa2*cos(t2)+a*w2^2*sin(t2)+b*w3^2*sin(t3)-c*w4^2*sin(t4)];

Y=Ja\acel;
alfa3=Y(1);
alfa4=Y(2);

M1=[t2*180/pi,t3*180/pi,t4*180/pi,w3,w4,alfa3,alfa4];

if chi==0
AM=M1;
chi=1;
else
AM=[AM;M1];
end
t2=t2+5*pi/180;

end

disp (' t2 t3 t4 w3 w4 alfa3

alfa4')
disp (AM);
T2=AM(:,1);
T3=AM(:,2);
T4=AM(:,3);
w3=AM(:,4);
w4=AM(:,5);
ALFA3=AM(:,6);
ALFA4=AM(:,7);

figure(1)
plot(T2,T3,T2,T4,'linewidth',1.0')
title('theta 3 y theta 4 en función de theta 2')
xlabel('theta 2')
ylabel('theta 3 y theta 4')
legend('curva theta 3','curva theta 4')
grid
figure(2)
plot(T2,w3,'k','linewidth',1.0')
hold on
plot(T2,w4,'r','linewidth',1.0')
grid on
title('Velocidades angulares W3 y W4 en función de theta 2')
xlabel('theta 2')
ylabel('Velocidades angulares W3 y W4')
legend('curva velocidad angular W3','curva velocidad angular W4')
figure(3)
plot(T2,ALFA3,'linewidth',1.0')
hold on
plot(T2,ALFA4,'r','linewidth',1.0')% ojo
grid on
title('Aceleraciones angulares alfa3 y alfa4 en función de theta 2')
xlabel('theta 2')
ylabel('Aceleraciones angulares alfa3 y alfa4')
legend('aceleración angular alfa3','curva aceleración angular alfa4')