%=============================================================
% TM350-Dinamica de Maquinas
% Prof. Jorge Luiz Erthal
% Análise geral e de ponto de interesse
% de um mecanismo biela-manivela
% Solução numérica da posição 
%=============================================================
clear all
close all
clc
%-------------------------------------------------------------
%% Dados numéricos
%-------------------------------------------------------------
R=0.25;         % Comprimento da manivela (m)                       <**>
C=0.1;          % Distancia do pistao ao centro da manivela (m)     <**>
L=0.45;         % comprimento da biela (m)                          <**>
qp=60;          % rotação de acionamento (rpm)                      <**>
qp=2*pi*qp/60;  % transformacao de rotacao para velocidade angular  <**>
qpp=0;          % aceleração de acionamento (rad/s^2)               <**>
%-------------------------------------------------------------
% ponto de interesse
up=L/2;             % coordenada local UP                           <**>
vp=0;               % coordenada local VP                           <**>
NP = 50;            % 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                <**>
%-------------------------------------------------------------
% matrizes de armazenamento
q=zeros(NP,1);
A=zeros(NP,1);
X=zeros(NP,1);
detJ=zeros(NP,1);
Ka=zeros(NP,1);
Kx=zeros(NP,1);
La=zeros(NP,1);
Lx=zeros(NP,1);
Xp=zeros(NP,1);
Yp=zeros(NP,1);
Kpx=zeros(NP,1);
Kpy=zeros(NP,1);
XPp=zeros(NP,1);
YPp=zeros(NP,1);
VP=zeros(NP,1);
Lpx=zeros(NP,1);
Lpy=zeros(NP,1);
XPpp=zeros(NP,1);
YPpp=zeros(NP,1);
acel=zeros(NP,1);
vel=zeros(NP,1);
%-----------------------------------------------------------------------
for i = 1:NP
    %-------------------------------------------------------------------
    %% Análise geral
    %-------------------------------------------------------------------
    q(i)=qmin+(i-1)*(qmax-qmin)/(NP-1); % valores de q nos NP pontos
    %-------------------------------------------------------------------
    %% 1 - posição
    %-------------------------------------------------------------------
    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
        %---------------------------------------------------------------
        % equacoes cinematicas de posicao                           <**>
        f = [R*cos(q(i))+L*cos(s(1))-s(2);
            R*sin(q(i))-L*sin(s(1))-C];
        %---------------------------------------------------------------
        % matriz jacobiana                                          <**>
        J = [-L*sin(s(1)) -1;
            -L*cos(s(1))  0];
        %---------------------------------------------------------------
        % novo valor do vetor secundario
        s=s-J\f;
        %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;
    %-------------------------------------------------------------------
    %% 2 - 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;
    %-------------------------------------------------------------------
    %% 3 - 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;
    %-------------------------------------------------------------------
    %% Ponto de interesse
    %-------------------------------------------------------------
    %% 4 - posição
    %-------------------------------------------------------------
    Ploc=[up;
          vp];
    O=[R*cos(q(i));
       R*sin(q(i))];
    Rot = [ cos(a) sin(a);
           -sin(a) cos(a)];
    Pglob=O+Rot*Ploc;
    Xp(i)=Pglob(1);
    Yp(i)=Pglob(2);
    %-------------------------------------------------------------
    %% 5 - velocidade
    %-------------------------------------------------------------
    dOdq = [-R*sin(q(i));
             R*cos(q(i))];
    dRotdq = [-sin(a)  cos(a);
              -cos(a) -sin(a)]*ka;
    KP = dOdq+dRotdq*Ploc;
    Kpx(i)=KP(1);
    Kpy(i)=KP(2);
    XPp(i)=Kpx(i)*qp;
    YPp(i)=Kpy(i)*qp;
    vel(i)=sqrt(XPp(i)^2+YPp(i)^2);
    %-------------------------------------------------------------
    %% 6 - aceleração
    d2Odq2 = [-R*cos(q(i));
              -R*sin(q(i))];
    d2Rotdq2 = [-cos(a)  -sin(a);
                 sin(a)  -cos(a)]*ka^2+...
               [-sin(a)   cos(a);
                -cos(a)  -sin(a)]*la;
    LP = d2Odq2+d2Rotdq2*Ploc;
    Lpx(i)=LP(1);
    Lpy(i)=LP(2);
    XPpp(i)=Kpx(i)*qpp+Lpx(i)*qp^2;
    YPpp(i)=Kpy(i)*qpp+Lpy(i)*qp^2;
    acel(i)=sqrt(XPpp(i)^2+YPpp(i)^2);
end
%-----------------------------------------------------------------------
%% Gráficos 
%-----------------------------------------------------------------------
%% 7 - análise geral (posicao, velocidade e aceleracao)
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])
%-------------------------------------------------------------
%% 8 - determinante da matriz jacobiana
%-------------------------------------------------------------
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])
%-------------------------------------------------------------
%% 9 - posição do ponto de interesse
%-------------------------------------------------------------
figure('Name','Análise de Ponto de Interesse')% título na moldura 
plot(Xp,Yp,...                       
    '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
%-------------------------------------------------------------
%% 10 - 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(3,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(3,2,2)                    % gráfico da direita (posição 2)
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])
% terceiro gráfico
subplot(3,2,3)                    % gráfico da esquerda (posição 1) 
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])
% quarto gráfico
subplot(3,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])
% quinto gráfico
subplot(3,2,5)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,VP,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Vel(q) ')                 % rótulo do eixo y
title('Velocidade de P')          % 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,acel,... 
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Acel(q) ')                % rótulo do eixo y
title('Aceleração de P')          % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% FIM
%-------------------------------------------------------------
