% solução dinâmica numérica de mecanismo de 4 barras
clc
clear all
close all

global L0 L1 L2 L3

L0=3.0; L1=1.0; L2=4.0; L3=2.5; % comprimentos de barras (m)

ts=[0 10]; % tempo
ci=[1.5708 0]; % condições iniciais

[t,teta]=ode45('nec4bar',ts,ci); % Runge-Kutta

% 1o. passo - coordenada generalizada

% visualização de variáveis de estado primárias em teta
figure(1)
subplot(2,1,1)
plot(t,teta(:,1),'r-')
ylabel('deslocamento angular (rad)')
xlabel('tempo (s)')
grid on
subplot(2,1,2)
plot(t,teta(:,2),'b-')
ylabel('velocidade angular (rad/s)')
xlabel('tempo (s)')
grid on

% 2o. passo - coordenadas auxiliares

% determinação de variáveis secundárias
  % deslocamentos angulares
k1=-2*L1*L3*sin(teta(:,1));
k2=2*L3*(L0-L1*cos(teta(:,1)));
k3=L0^2+L1^2-L2^2+L3^2-2*L0*L1*cos(teta(:,1));
fi(:,1)=wrapToPi(2*atan2(-k1-sqrt(k1.^2+k2.^2-k3.^2),k3-k2)); % barra 2
alfa(:,1)=atan2(-L1*sin(teta(:,1))+L3*sin(fi(:,1)),...
                L0-L1*cos(teta(:,1))+L3*cos(fi(:,1))); % barra 3
  % velocidades angulares
S1=(L1*sin(fi(:,1)-teta(:,1)))./(L2*sin(alfa(:,1)-fi(:,1)));
S2=(L1*sin(alfa(:,1)-teta(:,1)))./(L3*sin(alfa(:,1)-fi(:,1)));
fi(:,2)=S2(:,1).*teta(:,2);
alfa(:,2)=S1(:,1).*teta(:,1);

% visualização de variáveis de estado secundárias em alfa
figure(2)
subplot(2,1,1)
plot(t,alfa(:,1),'r-')
ylabel('deslocamento angular (rad)')
xlabel('tempo (s)')
grid on
subplot(2,1,2)
plot(t,alfa(:,2),'b-')
ylabel('velocidade angular (rad/s)')
xlabel('tempo (s)')
grid on
 
% visualização de variáveis de estado secundárias em fi
figure(3)
subplot(2,1,1)
plot(t,fi(:,1),'r-')
ylabel('deslocamento angular (rad)')
xlabel('tempo (s)')
grid on
subplot(2,1,2)
plot(t,fi(:,2),'b-')
ylabel('velocidade angular (rad/s)')
xlabel('tempo (s)')
grid on