%=============================================================
% TM350-Dinamica de Maquinas
% Prof. Jorge Luiz Erthal
% Análise geral e de ponto de interesse
% de um mecanismo de quatro barras
% Solução numérica da posição 
% Baseado no item 2.3 do livro
%=============================================================
clear all
close all
clc
%-------------------------------------------------------------
%% Dados numéricos
%-------------------------------------------------------------
C1=5;           % Comprimento do elo de entrada (pol)               <**>
C2=9;           % Comprimento do elo intermediario)                 <**>
C3=7;           % Comprimento do elo de saida                       <**>
C4=10;          % Comprimento do elo fixo                           <**>
qprpm=60;       % rotação de acionamento (rpm)                      <**>    
qp=2*pi*qprpm/60;  % transformacao de rotacao para velocidade angular  
qpp=0;          % aceleração de acionamento (rad/s^2)               <**>
%-------------------------------------------------------------
% ponto de interesse
up=C2/2;              % coordenada local UP                           <**>
vp=C2/2;               % coordenada local VP                           <**>
NP = 37;            % 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);
A2=zeros(NP,1);
A3=zeros(NP,1);
detJ=zeros(NP,1);
Ka2=zeros(NP,1);
Ka3=zeros(NP,1);
La2=zeros(NP,1);
La3=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);
vel=zeros(NP,1);
Lpx=zeros(NP,1);
Lpy=zeros(NP,1);
XPpp=zeros(NP,1);
YPpp=zeros(NP,1);
acel=zeros(NP,1);
%-----------------------------------------------------------------------
for i = 1:NP
    %-------------------------------------------------------------------
    %% Analise 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                    <**>
    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-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;
    %-------------------------------------------------------------------
    %% 2 - 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;
    %-------------------------------------------------------------------
    %% 3 - 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;
    %-------------------------------------------------------------------
    %% Ponto de interesse
    %-------------------------------------------------------------
    %% 4 - posição
    %-------------------------------------------------------------
    Ploc=[up;
          vp];
    O=[C1*cos(q(i));
       C1*sin(q(i))];
    Rot = [ cos(a2) -sin(a2);
            sin(a2)  cos(a2)];
    Pglob=O+Rot*Ploc;
    Xp(i)=Pglob(1);
    Yp(i)=Pglob(2);
    %-------------------------------------------------------------
    %% 5 - velocidade
    %-------------------------------------------------------------
    dOdq = [-C1*sin(q(i));
             C1*cos(q(i))];
    dRotdq = [-sin(a2)  cos(a2);
              -cos(a2) -sin(a2)]*ka2;
    KP = dOdq+dRotdq*Ploc;
    Kpx(i)=KP(1);
    Kpy(i)=KP(2);
    XPp(i)=Kpx(i)*qp;
    YPp(i)=Kpy(i)*qp;
    %-------------------------------------------------------------------
    % velocidade do ponto de interesse (Vp)
    vel(i)=sqrt(XPp(i)^2+YPp(i)^2);
    %-------------------------------------------------------------------
    %% 6 - aceleração
    d2Odq2 = [-C1*cos(q(i));
              -C1*sin(q(i))];
    d2Rotdq2 = [-cos(a2)   sin(a2);
                -sin(a2)  -cos(a2)]*ka2^2+...
               [-sin(a2)  -cos(a2);
                 cos(a2)  -sin(a2)]*la2;
    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;
    %-------------------------------------------------------------------
    % aceleração do ponto de interesse (Acel)
    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,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('Kx(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])
%-------------------------------------------------------------
%% 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,vel,...                       
    '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
%-------------------------------------------------------------
