%INGENIERIA MECANICA UNSAAC

%SOLUCIÓN DEL EXÁMEN

%SÍNTESIS
%DOCENTE ARTURO MACEDO SILVA
clear
clc
%Chevychev
%para tres posicionescos???(2j-1)/2n?
xo=1;%desde 1
xn=2;%hasta 2
n=3;%puntos de precisión
h=3;
h=1:h;
[XX(h)]=1/2*(xo+xn)-1/2*(xn-xo)*cos(pi*(2*h-1)/(2*n))%polinomio de
Chevychev

x1=XX(1);
x2=XX(2);
x3=XX(3);
%x4=XX(4);
X=[xo x1 x2 x3 xn]
% la función que se analiza
Yo=2*(xo)^(2)-2;
Y1=2*(x1)^(2)-2;
Y2=2*(x2)^(2)-2;
Y3=2*(x3)^(2)-2;
%Y4=2*(x4)^(2)-1;
Yn=2*(xn)^(2)-2;

Y=[ Yo Y1 Y2 Y3 Yn]

deltafi=60;%oscila arbitrario
deltax=xn-xo%xn-xo
deltasi=90;%oscila arbitrario
deltay=Yn-Yo%

Rfi=deltafi/deltax
Rsi=deltasi/deltay
fi21=Rfi*(x2-x1)
fi31=Rfi*(x3-x1)
%fi41=Rfi*(x4-x1)
si21=Rsi*(Y2-Y1)
si32=Rsi*(Y3-Y1)
%si42=Rsi*(Y4-Y1)
ang=[fi21 fi31 ;si21 si32 ]
%ecuación
fi=10
fi1=fi+Rfi*(x1-xo)
%fi1=105%dato inicial
fi2=Rfi*(x2-x1)+fi
fi3=Rfi*(x3-x1)+fi
%fi4=Rfi*(x4-x1)+fi

%ecuación
si=20%angulo de salida, inicial
si1=si+Rsi*(Y1-Yo)%66.27
%deltasi*(Y1-Yo)
si2=Rsi*(Y2-Y1)+si
si3=Rsi*(Y3-Y1)+si
%si4=Rsi*(Y4-Y1)+si

ANGULOS=[fi1 fi2 fi3 ;si1 si2 si3 ]

fi1=fi1*pi/180;
fi2=fi2*pi/180;
fi3=fi3*pi/180;

si1=si1*pi/180;
si2=si2*pi/180;
si3=si3*pi/180;

a=cos(fi1);
b=cos(fi2);
c=cos(fi3);
d=cos(si1);
e=cos(si2);
f=cos(si3);
w1=a-b;
w2=d-e;
w3=-cos(fi1-si1)+cos(fi2-si2);
w4=a-c;
w5=d-f;
w6=-cos(fi1-si1)+cos(fi3-si3);
K1=(w3*w5-w2*w6)/(w1*w5-w2*w4);
K2=(w1*w6-w3*w4)/(w1*w5-w2*w4);
%f1=fi o s1=si i=1,2 o 3
K3=-cos(fi1-si1)-K1*cos(fi1)-K2*cos(si1);

Z1=5;%arbirtrario
Z4=Z1/K1;
Z2=-Z1/K2;
Z3=sqrt(2*K3*Z2*Z4+Z1^2+Z2^2+Z4^2);
Z=[Z1 Z2 Z3 Z4]

S=[a d 1;b e 1;c f 1];

B=[-cos(fi1-si1);-cos(fi2-si2);-cos(fi3-si3)];
KKK=S\B;
v1=KKK(1);
v2=KKK(2);
v3=KKK(3);
Z1=Z1;%arbirtrario
Z4=Z1/v1;
Z2=-Z1/v2;
Z3=sqrt((2*v3*Z2*Z4)+Z1^2+Z2^2+Z4^2);
Z1=[Z1 Z2 Z3 Z4]

%------------------------------------------------

AM=[0,0,0,0,0,0,0,0,0,0,0,0,0,0];
chi=0;
 a=Z2; %longitud del eslabón 2
b=Z3; %longitud del eslabón 3
c=Z4; %longitud del eslabón 4
d=Z1; %longitud del eslabón 1
%angulos iniciales
t1=0*pi/180;
t2=35.9808*pi/180;%51.9615°,,,25.9808°,,,20°
t3=10*pi/180; %350°,,,,,,,350°,,135°
t4=60*pi/180; %arbitrario considere estos ángulos lo más reales
w2=0;%velocidad angular del eslabón2 dato
alfa2=0;%aceleración angular del eslabón 2 dato

for i=1:1
f1=a.*cos(t2)+b.*cos(t3)-c.*cos(t4)-d*cos(t1); %funciones de
generacion=0
f2=a.*sin(t2)+b.*sin(t3)-c.*sin(t4)-d*sin(t1); %funciones de
generacion=0

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);

e=0.00001;

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

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
%--------------------------------------------------------------

M1=[t2*180/pi,t4*180/pi];

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

end

disp (' t2 t4')

disp (AM);
T2=AM(:,1);
T4=AM(:,2);
figure(1)
plot(T2,T4)