clear all

close all
clc
%Analisis de posicion mecanismo Manivela-Biela-Invertida
% Longitudes de los eslabones
a=2 ;
c=2.5 ;
d=6 ;
Fi=90;

for TETA2=0:360
% Ecuaciones de posición
P = (a*sind(TETA2)*sind(Fi))+((a*cosd(TETA2))-d)*(cosd(Fi));
Q = -(a*sind(TETA2)*cosd(TETA2))+((a*cosd(TETA2))-d)*(sind(Fi));
R = -c*sind(Fi);

S= R-Q;
T= 2*P;
U= Q+R;

x= (-T + (sqrt(T^2-(4*S*U))))/(2*S);
TETA4=2*atand(x);
TETA3=TETA4+Fi;
b=((a*sind(TETA2))-(c*sind(TETA4)))/(sind(TETA3));

%Coordenadas de las juntas

O2X=0;
O2Y=0;
O4X=d;
O4Y=0;
AX=a*cosd(TETA2);
AY=a*sind(TETA2);
BX=d-c*cosd(180-TETA4);
BY=c*sind(180-TETA4);

%Crear los vectores a graficar

X=[O2X AX BX O4X];
Y=[O2Y AY BY O4Y];
subplot(1,1,1)
plot(X,Y);
vb=[BX BY]; %Vector de coordernadas de C
va=[AX AY]; %Vector de coordernadas de A
vo2=[O2X O2Y]; %Vector de coordernadas de O2
vo4=[O4X O4Y];%Vector de coordernadas de O4

B_circ=viscircles(vb,0.1); %Dibujo de circulos

A_circ=viscircles(va,0.1); %Dibujo de circulos
O2_circ=viscircles(vo2,0.1); %Dibujo de circulos
O4_circ=viscircles(vo4,0.1); %Dibujo de circulos
Atraj=viscircles([0 0],a,'LineStyle','--');
if TETA2==10 || TETA2==45 || TETA2==147 ||TETA2==230 ||TETA2==285
fprintf('\nTETA 2=%.2f',TETA2);
fprintf('\nTETA 3=%.2f',TETA3);
fprintf('\nTETA 4=%.2f',TETA4);
fprintf('\nB=%.2f',b);
fprintf('\n-----------------');
 end
axis equal
axis([-a-1 d+1 -a-1 a+2]);
pause(0.01)
delete(B_circ);
delete(A_circ);
delete(O2_circ);
delete(O4_circ);
end