%=============================================================
% TM350-Dinamica de Maquinas
% Prof. Jorge Luiz Erthal
% Aplicação do Princípio dos Trabalhos Virtuais
% Mecanismos: 4 barra
% Figura: quatrobarrasPTV.jpg
%=============================================================
clear all
close all
clc
%% apresentar gráficos
GRAFICO_GERAL = 'N';
GRAFICO_JACO = 'N';
GRAFICO_PONTO_DE_INTERESSE = 'S';
GRAFICO_FORCA_MOLA = 'S';
GRAFICO_MOMENTO_EQUILIBRANTE = 'S';
%-------------------------------------------------------------
%% 1 - Dados numéricos
%-------------------------------------------------------------
C1=5;           % Comprimento do elo de entrada (pol)   5            <**>
C2=9;           % Comprimento do elo intermediario)     9            <**>
C3=7;           % Comprimento do elo de saida           7            <**>
C4=10;          % Comprimento do elo fixo               10            <**>
C5=6;           % Altura do triângulo
%-------------------------------------------------------------
% ponto de interesse
up=0;              % ponto de aplicação da força            <**>
vp=C5;             
um=C2/3;               % centro de massa
vm=C5/3;
%-------------------------------------------------------------
% número de pontos e distribuição no intervalo
NP = 361;            % numero de pontos calculados
qmin = 0*pi/180;    % valor de q no inicio do intervalo             <**>
qmax = 360*pi/180;  % valor de q no fim do intervalo                <**>
%-----------------------------------------------------------------------
%-------------------------------------------------------------
% Força aplicada (N)
F = 10;
% Constante da mola (N/m)
km = 1;
% Comprimento livre da mola (m)
S0 = 0.15;
% massas (kg)
m1=0.45;        % massa do elo de entrada                        (kg) 0.45
m2=0.85;        % massa do elo intermediário                     (kg) 0.85
m3=0.4;         % massa do elo de saída                          (kg) 0.4
%-----------------------------------------------------------------------
% aceleração da gravidade (m/s^2)
grav = 9.81;
%-----------------------------------------------------------------------
%% INÍCIO DO LAÇO
%-----------------------------------------------------------------------
for i = 1:NP
    %% definição dos pontos
    q(i)=qmin+(i-1)*(qmax-qmin)/(NP-1); % valores de q nos NP pontos
    %-------------------------------------------------------------------
    %% análise geral de posição (solução numérica)
    %-------------------------------------------------------------------
    ef=10^(-6);         % tolerancia para o metodo de Newton Raphson
    %-------------------------------------------------------------------
    % valores iniciais das variaveis secundarias                    <**>
    a20 = 10*pi/180;     % variavel secundaria A2                    
    a30 = 60*pi/180;     % variavel secundaria A3    
    %-------------------------------------------------------------------
    % composicao dos valores iniciais do vetor secundario           <**>
    s=[a20; a30];         
    %-------------------------------------------------------------------
    % valor inicial da funçao f ( > ef )                            <**>
    f=[1;1];               
    %-------------------------------------------------------------------
    % iteracoes do metodo de Newton-Raphson
    while norm(f) > ef
        %---------------------------------------------------------------
        a2=s(1);
        a3=s(2);
        % equacoes cinematicas de posicao                           <**>
        f = [C1*cos(q(i))+C2*cos(a2)-C3*cos(a3)-C4;
             C1*sin(q(i))+C2*sin(a2)-C3*sin(a3)];
        %---------------------------------------------------------------
        % matriz jacobiana                                          <**>
        J = [-C2*sin(a2)  C3*sin(a3);
              C2*cos(a2) -C3*cos(a3)];
        %---------------------------------------------------------------
        % novo valor do vetor secundario
        s=s-inv(J)*f;
        %---------------------------------------------------------------
    end
    %-------------------------------------------------------------------
    % distribuição do resultado nas variáveis secundárias
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    a2=s(1);     
    a3=s(2);  
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    A2(i)=a2;     
    A3(i)=a3;
    %-------------------------------------------------------------------
    %% análise geral de velocidade
    %-------------------------------------------------------------------
    % matriz jacobiana                                              <**>
    J = [-C2*sin(a2)  C3*sin(a3);
          C2*cos(a2) -C3*cos(a3)];
    %-------------------------------------------------------------------
    % determinante da matriz jacobiana
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                
    detj = det(J);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                            
    detJ(i) = detj;
    %-------------------------------------------------------------------
    % derivada das equacoes cinematicas em relacao a q              <**>
    Q=[-C1*sin(q(i));
        C1*cos(q(i))];
    %-------------------------------------------------------------------
    % coeficientes de velocidade
    k=-inv(J)*Q;
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    ka2=k(1);
    ka3=k(2);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    Ka2(i)=ka2;
    Ka3(i)=ka3;
    %-------------------------------------------------------------------
    %% análise geral de aceleração
    %-------------------------------------------------------------------
    % derivada da matriz jacobiana em relacao a q                   <**>
    dJdq=[-C2*cos(a2)*ka2  C3*cos(a3)*ka3;
          -C2*sin(a2)*ka2  C3*sin(a3)*ka3];
    %-------------------------------------------------------------------
    % derivada do vetor primario em relacao a q                     <**>
    dQdq=[-C1*cos(q(i));                                            
          -C1*sin(q(i))];
    %-------------------------------------------------------------------
    % derivada dos coeficientes de velocidade
    l=-inv(J)*(dJdq*k+dQdq);
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    la2=l(1);
    la3=l(2);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    La2(i) = la2;
    La3(i) = la3;
    %-------------------------------------------------------------------
    %% análise de posição do ponto de interesse                                 <**>
    Xp(i)=C1*cos(q(i))+up*cos(a2)-vp*sin(a2);
    Yp(i)=C1*sin(q(i))+up*sin(a2)+vp*cos(a2);
    %-------------------------------------------------------------------
    %% coeficientes de velocidade do ponto de interesse (kp)         <**>
    Kpx(i)=-C1*sin(q(i))-up*sin(a2)*ka2-vp*cos(a2)*ka2;
    Kpy(i)= C1*cos(q(i))+up*cos(a2)*ka2-vp*sin(a2)*ka2;
    %-------------------------------------------------------------------
    %% derivadas dos coeficientes de velocidade do ponto de interesse (Lp)         <**>
    Lpx(i)=-C1*cos(q(i))-up*la2*sin(a2)-up*ka2^2*cos(a2)-vp*la2*cos(a2)+...
        vp*ka2^2*sin(a2);
    Lpy(i)=-C1*sin(q(i))+up*la2*cos(a2)-up*ka2^2*sin(a2)-vp*la2*sin(a2)-...
        vp*ka2^2*cos(a2);
    %-------------------------------------------------------------------
    %% análise de posição do centro de massa                              <**>
    Xm(i)=C1*cos(q(i))+um*cos(a2)-vm*sin(a2);
    Ym(i)=C1*sin(q(i))+um*sin(a2)+vm*cos(a2);
    %% coeficientes de velocidade do centro de massa  (km)         <**>
    Kmx(i)=-C1*sin(q(i))-um*sin(a2)*ka2-vm*cos(a2)*ka2;
    Kmy(i)= C1*cos(q(i))+um*cos(a2)*ka2-vm*sin(a2)*ka2;
    %-------------------------------------------------------------------
    %% derivadas dos coeficientes de velocidade do centro de massa (Lm)         <**>
    Lmx(i)=-C1*cos(q(i))-um*la2*sin(a2)-um*ka2^2*cos(a2)-vm*la2*cos(a2)+...
        vm*ka2^2*sin(a2);
    Lmy(i)=-C1*sin(q(i))+um*la2*cos(a2)-um*ka2^2*sin(a2)-vm*la2*sin(a2)-...
        vm*ka2^2*cos(a2);
    %-------------------------------------------------------------------
    %% Princípio dos trabalhos virtuais (NÃO ESTÁ FEITO!!!!!!!)
    %-------------------------------------------------------------
    % comprimento da mola
    S(i)=sqrt((C3*sin(a3))^2+(C4+C3*cos(a3))^2);
    ks=1/S(i)*(C3^2*sin(a3)*cos(a3)*ka3+(C4+C3*cos(a3))*(-C3*sin(a3)*ka3));
    % força na mola
    Fmola(i) = km*(S(i)-S0);
    % torque devido à ação da mola
    Tmola(i) = -km*(S(i)-S0)*ks;
    % torque devido à ação da força
    Tforca(i) = -F*Kpx(i);
    % torque devido à ação do peso
    Tpeso(i) = -grav*(m1*C1/2*cos(q(i))+m2*Kpy(i)+m3*C3/2*cos(a3)*ka3);
    % momento equilibrante
    T(i)=Tmola(i)+Tforca(i)+Tpeso(i);
end
%-----------------------------------------------------------------------
%% 3 - Gráficos da análise geral 
%-----------------------------------------------------------------------
%% 3.1 - Gráficos da análise geral (posicao, velocidade e aceleracao)
%-----------------------------------------------------------------------
if GRAFICO_GERAL == 'S'
figure('Name','Análise Geral')          % título na moldura superior
% gráficos geminados
% primeiro gráfico
subplot(3,2,1)                          % gráfico da esquerda (posição 1) 
plot(q*180/pi,A2*180/pi,...                                          <**>                       
    'LineWidth',2)                      % espessura da linha
grid on                                 % linhas de grade
xlabel('q (^o)')                        % rótulo do eixo x
ylabel('A2(q) (^o)')                    % rótulo do eixo y
title('Posição do elo intermediario')   % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% segundo gráfico
subplot(3,2,2)                          % gráfico da direita (posição 2)
plot(q*180/pi,A3*180/pi,...                                          <**>
    'r',...                             % cor da curva
    'LineWidth',2)                      % espessura da linha
grid on                                 % insere linhas de grade
xlabel('q (^o)')                        % rótulo do eixo x
ylabel('A3(q) (^0)')                    % rótulo do eixo y          <**>
title('Posição do elo de saida')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% terceiro gráfico
subplot(3,2,3)                          % gráfico da esquerda (posição 1) 
plot(q*180/pi,Ka2,...                                               <**>                       
    'LineWidth',2)                      % espessura da linha
grid on                                 % linhas de grade
xlabel('q (^o)')                        % rótulo do eixo x
ylabel('Ka2(q) ')                       % rótulo do eixo y          <**>
title('Coeficiente de velocidade Ka2')  % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% quarto gráfico
subplot(3,2,4)                    % gráfico da direita (posição 2)
plot(q*180/pi,Ka3,...                                               <**>   
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Ka3(q) ')                  % rótulo do eixo y                <**>
title('Coeficiente de velocidade Ka3')       % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% quinto gráfico
subplot(3,2,5)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,La2,...                                               <**>                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('La2(q) ')                 % rótulo do eixo y                <**>
title('Derivada do coeficiente de velocidade La2')   % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% sexto gráfico
subplot(3,2,6)                    % gráfico da direita (posição 2)
plot(q*180/pi,La3,...                                                <**>
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('La3(q) ')                 % rótulo do eixo y                <**>
title('Derivada do coeficiente de velocidade La3')   % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
end
%-------------------------------------------------------------
%% 3.2 - Gráfico do determinante da matriz jacobiana
%-------------------------------------------------------------
if GRAFICO_JACO == 'S'
figure('Name','Determinante da Matriz Jacobiana')% título na moldura 
plot(q*180/pi,detJ,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Det J(q)')                % rótulo do eixo y
title('Determinante da Matriz Jacobiana')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
end
%-------------------------------------------------------------
%% 3.3 - Gráficos da posição do ponto de interesse
%-------------------------------------------------------------
if GRAFICO_PONTO_DE_INTERESSE =='S'
figure('Name','Análise de Ponto de Interesse')% título na moldura 
plot(Xp,Yp,Xm,Ym,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('X (m)')                   % rótulo do eixo x
ylabel('Y (m)')                   % rótulo do eixo y
title('Posição')                  % título do gráfico
axis equal
%-------------------------------------------------------------
%% 3.4 - Gráficos da velocidade e aceleração do ponto de interesse
%-------------------------------------------------------------
figure('Name','Análise de Ponto de Interesse')% título na moldura superior
% gráficos geminados
% primeiro gráfico
subplot(2,2,1)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,Kpx,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Kpx(q)')                  % rótulo do eixo y
title('Coeficiente de velocidade Kpx')         % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% segundo gráfico
subplot(2,2,2)                    % gráfico da direita (posição 2)
plot(q*180/pi,Kpy,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Kpy(q) ')                 % rótulo do eixo y
title('Coeficiente de velocidade Kpy')       % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% terceiro gráfico
subplot(2,2,3)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,Lpx,...
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % insere linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Lpx(q) ')                 % rótulo do eixo y
title('Derivada do coeficiente de velocidade Lpx')   % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% quarto gráfico
subplot(2,2,4)                    % gráfico da direita (posição 2)
plot(q*180/pi,Lpy,...   
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Lpy(q) ')                 % rótulo do eixo y
title('Derivada do coeficiente de velocidade Lpy') % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
end
%-------------------------------------------------------------
%% Gráfico da força na mola
%-------------------------------------------------------------
if GRAFICO_FORCA_MOLA == 'S'
    figure('Name','PTV')% título na moldura
    plot(q*180/pi,Fmola,...
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('F_{mola} (N)')                % rótulo do eixo y
    title('Força na Mola')        % título do gráfico
    xlim([qmin*180/pi qmax*180/pi])
end
%-------------------------------------------------------------
%% Gráfico do momento equilibrante
%-------------------------------------------------------------
if GRAFICO_MOMENTO_EQUILIBRANTE == 'S'
    figure('Name','Momento equilibrante')% título na moldura
    plot(q*180/pi,Tmola,q*180/pi,Tforca,q*180/pi,Tpeso,q*180/pi,T,'LineWidth',2)
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('T (N.m)')                % rótulo do eixo y
    title('Momento Equilibrante')        % título do gráfico
    xlim([qmin*180/pi qmax*180/pi])
    legend('mola','força','peso','total')
end
%-------------------------------------------------------------
% FIM
%-------------------------------------------------------------

