clear all

close all
clc

%Analisis de posicion del mecanismo mbc

%Longitud de los eslabones

A = 2;
B = 8;
K = 4;
D = 4;
F = 5;
G = 4;
W2=-2*pi;
alpha_2=0;

for TETA2 = 0:360

%Ecuaciones de posicion lazo1
TETA3 = asind((-A*sind(TETA2))/B);
C = A*cosd(TETA2)+B*cosd(TETA3);

%Preparo cuadratica
k1 = (A^2+K^2+D^2+F^2-G^2)/(2*A*D);
k2 = (A*K*(cosd(TETA2)*cosd(TETA3)+sind(TETA2)*sind(TETA3))-
A*F*sind(TETA2)-K*F*sind(TETA3))/(A*D);

P = k1+k2-cosd(TETA2)-K*cosd(TETA3)/A;
Q = 2*sind(TETA2)+2*K*sind(TETA3)/A-2*F/A;
R = k1+k2+cosd(TETA2)+K*cosd(TETA3)/A;

X1 = (-Q-sqrt(Q^2-4*R*P))/(2*P);
TETA5 = 2*atand(X1);
TETA6 = acosd((A*cosd(TETA2)+K*cosd(TETA3)+D*cosd(TETA5))/G);

%Velocidad
VAx=W2*A;
W3=(A*W2*cosd(TETA2))/(B*cosd(TETA3));
VCx=-A*W2*sind(TETA2)+B*W3*sind(TETA3);
alpha_3=(A*alpha_2*cos(TETA2)-
A*W2*W2*sind(TETA2)+B*W3*W3*sind(TETA3))/(B*cosd(TETA3));
ACx=-A*alpha_2*sind(TETA2)-
A*W2*W2*cosd(TETA2)+B*alpha_3*sind(TETA3)+B*W3*W3*cosd(TETA3);

%Coordenadas de las juntas

O2X = 0;
O2Y = 0;
AX = A*cosd(TETA2);
AY = A*sind(TETA2);
CX = C;
CY = 0;
BX = AX+ K*cosd(TETA3);
BY = AY+ K*sind(TETA3);
DX = BX+D*cosd(TETA5);
 DY = BY+D*sind(TETA5);
O6X = 0;
O6Y = 5;

%Crear vectores a graficar

X = [O2X AX CX BX DX O6X];
Y = [O2Y AY CY BY DY O6Y];

subplot(2,1,1)
plot(X,Y)

Vo2 = [O2X O2Y];

Va = [AX AY];
Vb = [BX BY];
Vc = [CX CY];
Vd = [DX DY];
Vo6= [O6X O6Y];

if TETA2==74 || TETA2==135 || TETA2==230 || TETA2==285

TETA2
TETA3
C
TETA5
TETA6
end

axis equal
axis([-6 11 -A-1 6]);
pause(0.01)

subplot(2,3,4)
plot(TETA2,CX,'--rs','LineWidth',2)
title('Posición Cx vs TETA2')
hold on

subplot(2,3,5)
plot(TETA2,VCx,'--rs','LineWidth',2)
title('Velocidad C vs TETA2')
hold on

subplot(2,3,6)
plot(TETA2, ACx,'--rs','LineWidth',2)
title('Aceleraciòn corredera Cx vs TETA2')
hold on
end