%=============================================================
% TM350-Dinamica de Maquinas
% Prof. Jorge Luiz Erthal
% Aplicação do Princípio dos Trabalhos Virtuais
% Mecanismos: biela-manivela
% Figura: bielamanivelaPTV.jpg
%=============================================================
clear all
close all
clc
%% apresentar gráficos
GRAFICO_GERAL = 'N';
GRAFICO_JACO = 'N';
GRAFICO_PONTO_DE_INTERESSE = 'N';
GRAFICO_FORCA_MOLA = 'N';
GRAFICO_MOMENTO_EQUILIBRANTE = 'S';
%-------------------------------------------------------------
%% dados cinemática
%-------------------------------------------------------------
R  = 0.25;      % Comprimento da manivela (m)                       <**>
C  = 0.00;      % Distancia do pistao ao centro da manivela (m)     <**>
L  = 0.45;      % comprimento da biela (m)                          <**>
C1 = 1.00;      % Extremidade fixa da mola (m)                      <**>
%-------------------------------------------------------------
% pontos de interesse
up2=R/2;             % centro de massa da manivela                   <**>
vp2=0;               
up3=L/2;             % centro de massa da biela                      <**>
vp3=0;               
%-------------------------------------------------------------
% 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                <**>
%-----------------------------------------------------------------------
%% dados estática
%-------------------------------------------------------------
m2=0.01;    % massa da manivela (kg)
m3=0.01;    % massa da biela (kg)
m4=0.4;     % massa do pistao (kg)
%-------------------------------------------------------------
km = 2;     % constante da mola (N/m)
S0 = 0.2;   % comprimento livre da mola (m)
%-------------------------------------------------------------
F = 1;      % Força externa aplicada (N)
%-----------------------------------------------------------------------
grav = 9.81;        % aceleração da gravidade (m/s^2)
%-----------------------------------------------------------------------
%% 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                    <**>
    a0 = 10*pi/180;     % variavel secundaria A
    x0 = R+L;           % variavel secundaria X
    %-------------------------------------------------------------------
    % composicao dos valores iniciais do vetor secundario           <**>
    s=[a0; x0];
    %-------------------------------------------------------------------
    % valor inicial da funçao f ( > ef )                            <**>
    f=[1;1];
    %-------------------------------------------------------------------
    % iteracoes do metodo de Newton-Raphson
    while norm(f) > ef
        %---------------------------------------------------------------
        a=s(1);
        x=s(2);
        % equacoes cinematicas de posicao                           <**>
        f = [R*cos(q(i))+L*cos(a)-x;
            R*sin(q(i))-L*sin(a)-C];
        %---------------------------------------------------------------
        % matriz jacobiana                                          <**>
        J = [-L*sin(a) -1;
            -L*cos(a)  0];
        %---------------------------------------------------------------
        % novo valor do vetor secundario
        s=s-inv(J)*f;
        %---------------------------------------------------------------
    end
    %-------------------------------------------------------------------
    % distribuição do resultado nas variáveis secundárias
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    a=s(1);
    x=s(2);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    A(i)=a;
    X(i)=x;
    %-------------------------------------------------------------------
    %% análise geral de velocidade
    %-------------------------------------------------------------------
    % matriz jacobiana                                              <**>
    J=[-L*sin(a)   -1;
        -L*cos(a)    0];
    %-------------------------------------------------------------------
    % 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=[-R*sin(q(i));
        R*cos(q(i))];
    %-------------------------------------------------------------------
    % coeficientes de velocidade
    k=-inv(J)*Q;
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    ka=k(1);
    kx=k(2);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    Ka(i)=ka;
    Kx(i)=kx;
    %-------------------------------------------------------------------
    %% análise geral de aceleração
    %-------------------------------------------------------------------
    % derivada da matriz jacobiana em relacao a q                   <**>
    dJdq=[-ka*L*cos(a) 0;
        ka*L*sin(a) 0];
    %-------------------------------------------------------------------
    % derivada do vetor primario em relacao a q                     <**>
    dQdq=[-R*cos(q(i));
        -R*sin(q(i))];
    %-------------------------------------------------------------------
    % derivada dos coeficientes de velocidade
    l=-inv(J)*(dJdq*k+dQdq);
    %-------------------------------------------------------------------
    % para continuacao dos calculos                                 <**>
    la=l(1);
    lx=l(2);
    %-------------------------------------------------------------------
    % para armazenamento dos resultados                             <**>
    La(i) = la;
    Lx(i) = lx;
    %-----------------------------------------------------------------------
    %% análise de posição do CM da manivela                                <**>
    Loc2=[up2;
        vp2];
    Ori2=[0;
        0];
    Rot2=[cos(q(i)) -sin(q(i))
        sin(q(i))  cos(q(i))];
    Glob2=Ori2+Rot2*Loc2;
    xp2=Glob2(1);
    yp2=Glob2(2);
    %-------------------------------------------------------------------
    %% coeficientes de velocidade do CM da manivela         <**>
    dOri2dq = [0;
        0];
    dRot2dq = [-sin(q(i))  -cos(q(i));
        cos(q(i))  -sin(q(i))];
    Vel2=dOri2dq + dRot2dq*Loc2;
    kpx2=Vel2(1);
    kpy2=Vel2(2);
    %-------------------------------------------------------------------
    %% derivadas dos coeficientes de velocidade do CM da manivela         <**>
    ddOri2ddq = [0;
        0];
    ddRot2ddq = [-cos(q(i))  sin(q(i));
        -sin(q(i)) -cos(q(i))];
    Acel2=ddOri2ddq + ddRot2ddq*Loc2;
    lpx2=Acel2(1);
    lpy2=Acel2(2);
    %-------------------------------------------------------------------
    %% análise de posição do CM da biela                                <**>
    Loc3=[up3;
          vp3];
    Ori3=[R*cos(q(i));
        R*sin(q(i))];
    Rot3=[ cos(a) sin(a)
        -sin(a) cos(a)];
    Glob3=Ori3+Rot3*Loc3;
    xp3=Glob3(1);
    yp3=Glob3(2);
    %-------------------------------------------------------------------
    %% coeficientes de velocidade do CM da biela         <**>
    dOri3dq = [-R*sin(q(i));
        R*cos(q(i))];
    dRot3dq = [-sin(a)*ka  cos(a)*ka;
        -cos(a)*ka -sin(a)*ka];
    Vel3=dOri3dq + dRot3dq*Loc3;
    kpx3=Vel2(1);
    kpy3=Vel2(2);
    %-------------------------------------------------------------------
    %% derivadas dos coeficientes de velocidade do CM da biela         <**>
    ddOri3ddq = [-R*cos(q(i));
        -R*sin(q(i))];
    ddRot3ddq = [-cos(a)*ka^2-sin(a)*la -sin(a)*ka^2+cos(a)*la;
        sin(a)*ka^2-cos(a)*la -cos(a)*ka^2-sin(a)*la];
    Acel3=ddOri3ddq + ddRot3ddq*Loc3;
    lpx3=Acel3(1);
    lpy3=Acel3(2);
    %-------------------------------------------------------------------
    %% Princípio dos trabalhos virtuais
    %-------------------------------------------------------------
    % força na mola
    Fmola(i) = km*(C1-x-S0);
    % torque devido à ação da mola
    Tmola(i) = -km*(C1-x-S0)*kx;
    % torque devido à ação da força
    Tforca(i) = -F*kx;
    % torque devido à ação do peso
    Tpeso(i) = m2*grav*kpy2 + m3*grav*kpy3;
    % momento equilibrante
    T(i)=Tmola(i)+Tforca(i)+Tpeso(i);
end
%-----------------------------------------------------------------------
%% 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,A*180/pi,...                                          <**>
        'LineWidth',2)                      % espessura da linha
    grid on                                 % linhas de grade
    xlabel('q (^o)')                        % rótulo do eixo x
    ylabel('A(q) (^o)')                     % rótulo do eixo y
    title('Posição da biela')               % 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,X,...                                                 <**>
        'r',...                             % cor da curva
        'LineWidth',2)                      % espessura da linha
    grid on                                 % insere linhas de grade
    xlabel('q (^o)')                        % rótulo do eixo x
    ylabel('X(q) (mm)')                     % rótulo do eixo y          <**>
    title('Posição do pistão')              % 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,Ka,...                                                <**>
        'LineWidth',2)                      % espessura da linha
    grid on                                 % linhas de grade
    xlabel('q (^o)')                        % rótulo do eixo x
    ylabel('Ka(q) ')                        % rótulo do eixo y          <**>
    title('Coeficiente de velocidade Ka')   % 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,Kx,...                                                <**>
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Kx(q) ')                  % rótulo do eixo y                <**>
    title('Coeficiente de velocidade Kx')       % 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,La,...                                                <**>
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('La(q) ')                  % rótulo do eixo y                <**>
    title('Derivada do coeficiente de velocidade La')    % 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,Lx,...                                                <**>
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Lx(q) ')                  % rótulo do eixo y                <**>
    title('Derivada do coeficiente de velocidade Lx')    % título do gráfico
    xlim([qmin*180/pi qmax*180/pi])
end
%-------------------------------------------------------------
%% 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
%-------------------------------------------------------------
%% Gráficos da posição dos pontos de interesse
%-------------------------------------------------------------
if GRAFICO_PONTO_DE_INTERESSE =='S'
    figure('Name','Análise dos Pontos de Interesse')% título na moldura
    plot(Xp2,Yp2,Xp3,Yp3,...
        '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
    legend('P2','P3')
    %-------------------------------------------------------------
    %% Gráficos dos coeficientes de velocidade e de aceleração
    %-------------------------------------------------------------
    figure('Name','Análise do CM da Manivela (P2)')% 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,Kpx2,...
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Kpx2(q)')                  % rótulo do eixo y
    title('Coeficiente de velocidade Kpx2')         % 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,Kpy2,...
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % insere linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Kpy2(q) ')                 % rótulo do eixo y
    title('Coeficiente de velocidade Kpy2')   % 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,Lpx2,...
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Lpx2(q) ')                 % rótulo do eixo y
    title('Deriada do coeficiente de velocidade Lpx2')       % 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,Lpy2,...
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Lpy2(q) ')                 % rótulo do eixo y
    title('Derivada do coeficiente de velocidade Lpy2') % título do gráfico
    xlim([qmin*180/pi qmax*180/pi])
    %-------------------------------------------------------------
    figure('Name','Análise do CM da Biela (P3)')% 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,Kpx3,...
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Kpx3(q)')                  % rótulo do eixo y
    title('Coeficiente de velocidade Kpx3')         % 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,Kpy3,...
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % insere linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Kpy3(q) ')                 % rótulo do eixo y
    title('Coeficiente de velocidade Kpy3')   % 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,Lpx3,...
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Lpx3(q) ')                 % rótulo do eixo y
    title('Deriada do coeficiente de velocidade Lpx3')       % 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,Lpy3,...
        'r',...                       % cor da curva
        'LineWidth',2)                % espessura da linha
    grid on                           % linhas de grade
    xlabel('q (^o)')                  % rótulo do eixo x
    ylabel('Lpy3(q) ')                 % rótulo do eixo y
    title('Derivada do coeficiente de velocidade Lpy3') % 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
%-------------------------------------------------------------
