%=============================================================
% Mecanismos
% Prof. Jorge Luiz Erthal
% Análise estática do mecanismo do exercício 7.1 - Doughty
% Solução da posição numérica
%=============================================================
clear all
close all
clc
%-------------------------------------------------------------
%% 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;
%-------------------------------------------------------------
% ESTÁTICA
grav = 9.81;    % m/s^2
m1=2;       % kg
m2=2;       % kg
m3=5;       % kg
I3=0.05;    % kg.m^2
%-------------------------------------------------------------
kmola = 50; % N/m
S0 = C1/2;  % m
%-------------------------------------------------------------
Fx = 60;    % N
Fy = 40;    % N
%-------------------------------------------------------------
%% Análises
%-------------------------------------------------------------
% número de pontos
NP = 25;
%-------------------------------------------------------------
% intervalo de funcionamento
qmin = -0.8*C1;
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);
vetLpx1=zeros(NP,1);
vetLpy1=zeros(NP,1);

vetXp2=zeros(NP,1);
vetYp2=zeros(NP,1);
vetKpx2=zeros(NP,1);
vetKpy2=zeros(NP,1);
vetLpx2=zeros(NP,1);
vetLpy2=zeros(NP,1);

XR=zeros(NP,1);
YR=zeros(NP,1);
torque=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];
        s=s-jaco\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 = -jaco\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 = -jaco\(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 - ponto de aplicação das forças
    %-------------------------------------------------------------
    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;   
    Xp1 = Pglob(1);
    Yp1 = 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;
    Kpx1 = Kp(1);
    Kpy1 = 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;
    Lpx1 = Lp(1);
    Lpy1 = Lp(2);    
    %-------------------------------------------------------------
    vetXp1(i)=Xp1;
    vetYp1(i)=Yp1;
    %-------------------------------------------------------------
    vetKpx1(i)=Kpx1;
    vetKpy1(i)=Kpy1;
    %-------------------------------------------------------------
    vetLpx1(i)=Lpx1;
    vetLpy1(i)=Lpy1;
    %-------------------------------------------------------------
    %% 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;   
    Xp2 = Pglob(1);
    Yp2 = 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;
    Kpx2 = Kp(1);
    Kpy2 = 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;
    Lpx2 = Lp(1);
    Lpy2 = Lp(2);    
    %-------------------------------------------------------------
    vetXp2(i)=Xp2;
    vetYp2(i)=Yp2;
    %-------------------------------------------------------------
    vetKpx2(i)=Kpx2;
    vetKpy2(i)=Kpy2;
    %-------------------------------------------------------------
    vetLpx2(i)=Lpx2;
    vetLpy2(i)=Lpy2;
    %-------------------------------------------------------------
    %% ESTÁTICA
    %-------------------------------------------------------------
    vetTmola(i)=-kmola*(X-S0)*Kx/Ka;
    vetTgravBloco(i)=-m1*grav/Ka;
    vetTgravElo(i)=-m3*grav*Kpy2/Ka;
    vetTfx(i)=Fx*Kpx1/Ka;
    vetTfy(i)=Fy*Kpy1/Ka;
    vetTorque(i)=vetTmola(i)+vetTgravBloco(i)+vetTgravElo(i)+vetTfx(i)+vetTfy(i);
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])
%-------------------------------------------------------------
%% Gráfico do torque de acionamento
%-------------------------------------------------------------
figure('Name','Torque de acionamento')
plot(vetq,vetTorque,'b','LineWidth',2)
hold on
plot(vetq,vetTmola,'r','LineWidth',1)
hold on
plot(vetq,vetTgravBloco,'--r','LineWidth',1)
hold on
plot(vetq,vetTgravElo,'b','LineWidth',1)
hold on
plot(vetq,vetTfx,'--b','LineWidth',1)
hold on
plot(vetq,vetTfy,'+-b','LineWidth',1)
grid on
xlabel('q (m)')
ylabel('Torque (N.m)')
title('Torque de acionamento')
xlim([qmin qmax])
legend('Torque total','Mola','Grav. Bloco','Grav. Elo','F_x','F_y')
%-------------------------------------------------------------
%% FIM
%-------------------------------------------------------------
