%=============================================================
% Mecanismos
% Prof. Jorge Luiz Erthal
% Análise cinemática geral e de ponto de interesse
% do mecanismo do exercício 7.1 - Doughty
% Solução da posição numérica
%=============================================================
clear all
close all
clc




%return
%-------------------------------------------------------------
%% dados numéricos
%-------------------------------------------------------------
disp('Dados Numéricos')
clc
%-------------------------------------------------------------
% dimensões dos elos
C1=0.3;
C2=0.2;
%-------------------------------------------------------------
% pontos de interesse
% ponto de aplicação das forças
UP1=C1;
VP1=C2;
% centro de massa do elo triangular
UP2=C1*2/3;
VP2=C2/3;
%-------------------------------------------------------------
% Análises
%-------------------------------------------------------------
% número de pontos
NP = 25;
%-------------------------------------------------------------
% intervalo de funcionamento
qmin = 0;
qmax = 0.8*C1;
%-------------------------------------------------------------
%% dimensionamento dos vetores
vetq=zeros(NP);
vetA=zeros(NP,1);
vetX=zeros(NP,1);
vetdetjaco=zeros(NP,1);
vetKa=zeros(NP,1);
vetKx=zeros(NP,1);
vetLa=zeros(NP,1);
vetLx=zeros(NP,1);
vetXp1=zeros(NP,1);
vetYp1=zeros(NP,1);
vetKpx1=zeros(NP,1);
vetKpy1=zeros(NP,1);
vetVp=zeros(NP,1);
vetLpx1=zeros(NP,1);
vetLpy1=zeros(NP,1);
vetAcel=zeros(NP,1);
XR=zeros(NP,1);
YR=zeros(NP,1);
%-------------------------------------------------------------
for i = 1:NP
    vetq=linspace(qmin,qmax,NP);
    %-------------------------------------------------------------
    %% análise geral de posição
    %-------------------------------------------------------------
    ZERO=10^(-6);     % tolerancia para o metodo de Newton Raphson
    a0 = 0*pi/180;   % valores iniciais das variaveis secundarias
    x0 = 0;
    s=[a0; x0];
    f=[1;1];          %funçao f cuja norma e maior que ZERO
    while norm(f) > ZERO
        q = vetq(i);
        A = s(1);
        X = s(2);
        f = [ C1*cos(A)-X;
              C1*sin(A)-q];
        jaco=[ -C1*sin(A), -1;
                C1*cos(A),  0];
        jacoinv=inv(jaco);
        s=s-jacoinv*f;
    end
    % distribuição do resultado nas variáveis secundárias
    A=s(1); 
    X=s(2);
    vetA(i)=A;     % armazenamento no vetor
    vetX(i)=X;     % armazenamento no vetor
    %-------------------------------------------------------------
    %% velocidade
    %-------------------------------------------------------------
    Q = [ 0;
         -1];
    K = -jacoinv*Q;
    Ka = K(1);
    Kx = K(2);
    % determinante da matriz jacobiana
    vetdetjaco(i) = det(jaco);
    % coeficientes de velocidade
    vetKa(i)=Ka;           % armazenamento no vetor
    vetKx(i)=Kx;           % armazenamento no vetor
    %-------------------------------------------------------------
    %% aceleração
    %-------------------------------------------------------------
    dJdq = [-C1*cos(A) 0;
            -C1*sin(A) 0];
    dQdq = [0;
            0];
    L = -jacoinv*(dJdq*K+dQdq);
    La = L(1);
    Lx = L(2);    
    % derivadas dos coeficientes de velocidade
    vetLa(i)=La;           % armazenamento no vetor
    vetLx(i)=Lx;           % armazenamento no vetor
    %-------------------------------------------------------------
    %% Ponto de interesse 1
    %-------------------------------------------------------------
    UP=UP1;
    VP=VP1;
    %-------------------------------------------------------------
    % origem do sistema local
    O = [0;             
	     q];
    % matriz de rotação
    rot = [ cos(A)  sin(A)       
          -sin(A)  cos(A)];
    % coordenadas locais do ponto de interesse
    Ploc = [UP; VP];     
    % coordenadas globais do ponto de interesse
    Pglob = O+rot*Ploc;   
    Xp = Pglob(1);
    Yp = Pglob(2);
    %----------------------------------------------------
    % Velocidade
    % derivada da posição da origem do sistema local
    dOdq = [0;
            1];
    % derivada da matriz de rotação
    dRdq = [-sin(A)*Ka  cos(A)*Ka;
            -cos(A)*Ka -sin(A)*Ka];
    % coeficientes de velocidade
    Kp = dOdq + dRdq*Ploc;
    Kpx = Kp(1);
    Kpy = Kp(2);
    %-------------------------------------------------------------
    % Aceleração
    %-------------------------------------------------------------
    % derivada segunda da posição da origem do sistema local
    ddOddq = [0;
              0];
    % derivada segunda da matriz de rotação
    ddRddq = [-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];
    % derivada dos coeficientes de velocidade
    Lp = ddOddq + ddRddq*Ploc;
    Lpx = Lp(1);
    Lpy = Lp(2);    
    %-------------------------------------------------------------
    vetXp1(i)=Xp;
    vetYp1(i)=Yp;
    %-------------------------------------------------------------
    vetKpx1(i)=Kpx;
    vetKpy1(i)=Kpy;
    %-------------------------------------------------------------
    vetLpx1(i)=Lpx;
    vetLpy1(i)=Lpy;
    %-------------------------------------------------------------
    %% Ponto de interesse 2
    %-------------------------------------------------------------
    UP=UP2;
    VP=VP2;
    %-------------------------------------------------------------
    % origem do sistema local
    O = [0;             
	     q];
    % matriz de rotação
    rot = [ cos(A)  sin(A)       
           -sin(A)  cos(A)];
    % coordenadas locais do ponto de interesse
    Ploc = [UP; VP];     
    % coordenadas globais do ponto de interesse
    Pglob = O+rot*Ploc;   
    Xp = Pglob(1);
    Yp = Pglob(2);
    %----------------------------------------------------
    % Velocidade
    % derivada da posição da origem do sistema local
    dOdq = [0;
            1];
    % derivada da matriz de rotação
    dRdq = [-sin(A)*Ka  cos(A)*Ka;
            -cos(A)*Ka -sin(A)*Ka];
    % coeficientes de velocidade
    Kp = dOdq + dRdq*Ploc;
    Kpx = Kp(1);
    Kpy = Kp(2);
    %-------------------------------------------------------------
    % Aceleração
    %-------------------------------------------------------------
    % derivada segunda da posição da origem do sistema local
    ddOddq = [0;
              0];
    % derivada segunda da matriz de rotação
    ddRddq = [-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];
    % derivada dos coeficientes de velocidade
    Lp = ddOddq + ddRddq*Ploc;
    Lpx = Lp(1);
    Lpy = Lp(2);    
    %-------------------------------------------------------------
    vetXp2(i)=Xp;
    vetYp2(i)=Yp;
    %-------------------------------------------------------------
    vetKpx2(i)=Kpx;
    vetKpy2(i)=Kpy;
    %-------------------------------------------------------------
    vetLpx2(i)=Lpx;
    vetLpy2(i)=Lpy;
    %-------------------------------------------------------------
end
%-------------------------------------------------------------
%% Gráficos da análise geral
%-------------------------------------------------------------
figure('Name','Análise Geral')    % título na moldura superior
% gráficos geminados
% primeiro gráfico
subplot(3,2,1)
plot(vetq,vetA*180/pi,'LineWidth',2)
grid on 
xlabel('q (m)')
ylabel('A(q) (^o)')
title('Posição do elo intermediário')
xlim([qmin qmax])
% segundo gráfico
subplot(3,2,2)
plot(vetq,vetX,'r','LineWidth',2)
grid on 
xlabel('q (m)')
ylabel('X(q) (m)')
title('Posição do bloco horizontal')
xlim([qmin qmax])
% terceiro gráfico
subplot(3,2,3)
plot(vetq,vetKa,'LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Ka(q) ') 
title('Coeficiente de velocidade Ka')
xlim([qmin qmax])
% quarto gráfico
subplot(3,2,4)
plot(vetq,vetKx,'r','LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Kx(q) ')
title('Coeficiente de velocidade Kx')
xlim([qmin qmax])
% quinto gráfico
subplot(3,2,5)
plot(vetq,vetLa,'LineWidth',2)
grid on 
xlabel('q (m)')
ylabel('La(q) ')
title('Derivada do coeficiente de velocidade La')
xlim([qmin qmax])
% sexto gráfico
subplot(3,2,6)
plot(vetq,vetLx,'r','LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Lx(q) ')    
title('Derivada do coeficiente de velocidade Lx')
xlim([qmin qmax])
%-------------------------------------------------------------
%% Gráfico do determinante da matriz jacobiana
%-------------------------------------------------------------
figure('Name','Determinante da Matriz Jacobiana')% título na moldura 
plot(vetq,vetdetjaco,'LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Det J(q)')
title('Determinante da Matriz Jacobiana')
xlim([qmin qmax])
%-------------------------------------------------------------
%% Gráfico da posição do ponto de interesse
%-------------------------------------------------------------
figure('Name','Posição dos Pontos de Interesse')% título na moldura 
plot(vetXp1,vetYp1,'r-','LineWidth',2)
hold on
plot(vetXp2,vetYp2,'b-','LineWidth',2)
grid on
axis equal
xlabel('X (m)')
ylabel('Y (m)')
xlim([0 inf])
ylim([0 inf])
title('Posição')
%-------------------------------------------------------------
%% Gráficos da velocidade e aceleração do ponto de interesse
%-------------------------------------------------------------
figure('Name','Velocidade e aceleração do Ponto de Interesse')
% gráficos geminados
% primeiro gráfico
subplot(2,2,1)
plot(vetq,vetKpx1,'b','LineWidth',2)
hold on
plot(vetq,vetKpx2,'r','LineWidth',2)
grid on
legend('Ponto da força','Centro de massa')
xlabel('q (m)')
ylabel('Kpx(q)')
title('Coeficiente de velocidade Kpx')
xlim([qmin qmax])
% segundo gráfico
subplot(2,2,2)
plot(vetq,vetKpy1,'b','LineWidth',2)
hold on
plot(vetq,vetKpy2,'r','LineWidth',2)
grid on 
xlabel('q (m)')
ylabel('Kpy(q) ')
title('Coeficiente de velocidade Kpy')
xlim([qmin qmax])
% terceiro gráfico
subplot(2,2,3)
plot(vetq,vetLpx1,'b','LineWidth',2)
hold on
plot(vetq,vetLpx2,'r','LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Lpx(q) ')
title('Derivada do coeficiente de velocidade Lpx')
xlim([qmin qmax])
% quarto gráfico
subplot(2,2,4)
plot(vetq,vetLpy1,'b','LineWidth',2)
hold on
plot(vetq,vetLpy2,'r','LineWidth',2)
grid on
xlabel('q (m)')
ylabel('Lpy(q) ')
title('Derivada do coeficiente de velocidade Lpy')
xlim([qmin qmax])









%-------------------------------------------------------------
% FIM
%-------------------------------------------------------------
