//====================================================================
// TMEC025-Mecanismos
// Prof. Jorge Luiz Erthal
// Trabalho de Mecanismos - Etapa 6
// Análise de Pontos de Interesse
// Título do Projeto: Mecanismo de 4 barras
// Equipe no.:
// Componentes da equipe:
// 1- 
// 2- 
// 3-
// 4-
//====================================================================
//clc     // limpa o console
clear   // apaga todas as variáveis
xdel(winsid())   // fecha todas as janelas gráficas
//-------------------------------------------------------------
//// 1 - Dados numéricos
//-------------------------------------------------------------
C1=6;    // comprimento do elo fixo
C2=5;    // comprimento do elo de entrada
C3=2;    // comprimento do elo intermediário
C4=4;    // comprimento do elo de saída
//-------------------------------------------------------------
Up1=C3/2;
Vp1=C3/2;
Up2=C4/2;
Vp2=C4/2;
//-------------------------------------------------------------
NP=37;   // Número de pontos dos gráficos
// ponto escolhido para desenhar a cadeia cinemática:
PTO=30;  // escolher um número inteiro entre 1 e NP
//-------------------------------------------------------------
// Intervalo de funcionamento
qmin = 26*%pi/180;    // valor inicial da variável primária
qmax = 60*%pi/180;  // valor final da variável primária
q=linspace(qmin,qmax,NP); // gera NP valores para q entre qmin e qmax
//-------------------------------------------------------------
ZERO=10^(-6);       // tolerancia para o metodo de Newton Raphson
//-------------------------------------------------------------
// valores aproximados das variaveis secundarias na posição inicial qmin
a = 60*%pi/180;
b = 90*%pi/180;
// vetor das posições secundárias
s=[a; b];
//-------------------------------------------------------------
// Início dos cálculos
for i = 1:NP
    //-------------------------------------------------------------
    // 2 - Análise geral de posição
    //-------------------------------------------------------------
    calcula='sim';
    while calcula == 'sim'
        // equações cinemáticas de posição
        f = [C2*cos(q(i))+C3*cos(a)-C4*cos(b)-C1;
             C2*sin(q(i))+C3*sin(a)-C4*sin(b)];
        // matriz jacobiana
        J = [-C3*sin(a)  C4*sin(b);
              C3*cos(a) -C4*cos(b)];
        if norm(f) <= ZERO
            calcula = 'nao';
        else
            // nova posição
            s=s-inv(J)*f;
            // separação das variáveis
            a=s(1);
            b=s(2);
        end
    end
    // armazenamento do resultado nas variáveis secundárias
    A(i)=s(1);
    B(i)=s(2);
    // determinante da matriz jacobiana
    detJ(i) = det(J);
    // -------------------------------------------------------------
    // 3 - Análise geral de velocidades
    // -------------------------------------------------------------
    // derivada das equacoes cinematicas em relacao a q
    Q=[-C2*sin(q(i));
        C2*cos(q(i))];
    // coeficientes de velocidade
    k=-inv(J)*Q;
    Ka(i)=k(1);
    Kb(i)=k(2);
    //-------------------------------------------------------------
    // 4 - Análise geral de aceleração
    //-------------------------------------------------------------
    // derivadas dos coeficientes de velocidade
    dJdq=[-C3*cos(A(i))*Ka(i)  C4*cos(B(i))*Kb(i);
          -C3*sin(A(i))*Ka(i)  C4*sin(B(i))*Kb(i)];
    dQdq=[-C2*cos(q(i));
          -C2*sin(q(i))];
    l=-inv(J)*(dJdq*k+dQdq);
    La(i) = l(1);
    Lb(i) = l(2);
    //-------------------------------------------------------------
    //// 5.1 - Ponto de interesse 1
    //-------------------------------------------------------------
    // Posição
    //-------------------------------------------------------------
    Ploc=[Up1;
          Vp1];
    O=[C2*cos(q(i));
       C2*sin(q(i))];
    Rot = [ cos(A(i)) -sin(A(i));
            sin(A(i))  cos(A(i))];
    Pglob=O+Rot*Ploc;
    Xp1(i)=Pglob(1);
    Yp1(i)=Pglob(2);
    //-------------------------------------------------------------
    // Velocidade 
    //-------------------------------------------------------------
    dOdq = [-C2*sin(q(i));
             C2*cos(q(i))];
    dRotdq = [-sin(A(i))*Ka(i) -cos(A(i))*Ka(i);
               cos(A(i))*Ka(i) -sin(A(i))*Ka(i)];
    KP = dOdq+dRotdq*Ploc;
    Kpx1(i)=KP(1);
    Kpy1(i)=KP(2);
    //-------------------------------------------------------------
    // Aceleração 
    //-------------------------------------------------------------
    d2Odq2 = [-C2*cos(q(i));
              -C2*sin(q(i))];
    d2Rotdq2 = [-cos(A(i))*Ka(i)^2-sin(A(i))*La(i)  sin(A(i))*Ka(i)^2-cos(A(i))*La(i) ;
                -sin(A(i))*Ka(i)^2+cos(A(i))*La(i) -cos(A(i))*Ka(i)^2-sin(A(i))*La(i)];
    LP = d2Odq2+d2Rotdq2*Ploc;
    Lpx1(i)=LP(1);
    Lpy1(i)=LP(2);
    //-------------------------------------------------------------
    //// 5.2 - Ponto de interesse 2
    //-------------------------------------------------------------
    // Posição
    //-------------------------------------------------------------
    Ploc=[Up2;
          Vp2];
    O=[C1;
       0];
    Rot = [ cos(B(i)) -sin(B(i));
            sin(B(i))  cos(B(i))];
    Pglob=O+Rot*Ploc;
    Xp2(i)=Pglob(1);
    Yp2(i)=Pglob(2);
    //-------------------------------------------------------------
    // Velocidade 
    //-------------------------------------------------------------
    dOdq = [0;
            0];
    dRotdq = [-sin(B(i))*Kb(i) -cos(B(i))*Kb(i);
               cos(B(i))*Kb(i) -sin(B(i))*Kb(i)];
    KP = dOdq+dRotdq*Ploc;
    Kpx2(i)=KP(1);
    Kpy2(i)=KP(2);
    //-------------------------------------------------------------
    // Aceleração 
    //-------------------------------------------------------------
    d2Odq2 = [0;
              0];
    d2Rotdq2 = [-cos(B(i))*Kb(i)^2-sin(B(i))*Lb(i)  sin(B(i))*Kb(i)^2-cos(B(i))*Lb(i) ;
                -sin(B(i))*Kb(i)^2+cos(B(i))*Lb(i)  -cos(B(i))*Kb(i)^2-sin(B(i))*Lb(i)];
    LP = d2Odq2+d2Rotdq2*Ploc;
    Lpx2(i)=LP(1);
    Lpy2(i)=LP(2);
end
//-------------------------------------------------------------
// 6 - GRÁFICOS DAS VARIÁVEIS SECUNDÁRIAS
//-------------------------------------------------------------
scf(1)
subplot(321)
plot(q'*180/%pi,A*180/%pi,'r-')
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('A(q) (grau)','fontsize', 3);
title('Posição do elo intermediário','color','black','fontsize',3);
//-------------------------------------------------------------
subplot(322)
plot(q'*180/%pi,B*180/%pi)
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('B(q) (grau)', 'fontsize', 3);
title('Posição do elo de saída','color','black','fontsize',3);
//-------------------------------------------------------------
subplot(323)
plot(q'*180/%pi,Ka,'r-')
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('K_a(q)', 'fontsize', 3);
title('Coeficiente de velocidade K_a','color','black','fontsize',3);
subplot(324)
plot(q'*180/%pi,Kb)
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('K_b(q)', 'fontsize', 3);
title('Coeficiente de velocidade K_b','color','black','fontsize',3);
//-------------------------------------------------------------
subplot(325)
plot(q'*180/%pi,La,'r-')
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('L_a(q)', 'fontsize', 3);
title('Derivada do coeficiente de velocidade L_a','color','black','fontsize',3);
//-------------------------------------------------------------
subplot(326)
plot(q'*180/%pi,Lb)
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('L_b(q)', 'fontsize', 3);
title('Derivada do coeficiente de velocidade L_b','color','black','fontsize',3);
//-------------------------------------------------------------
// GRÁFICO DO DETERMINANTE DA MATRIZ JACOBIANA
scf(2)
plot(q'*180/%pi,detJ)
set(gca(),'grid',[1 1])
xlabel('q (grau)', 'fontsize', 3);
ylabel('Det J', 'fontsize', 3);
title('Determinante da matriz Jacobiana','color','black','fontsize',3);
//-------------------------------------------------------------
//// 10 - Gráficos das compnentes
//-------------------------------------------------------------
scf(3)
//-------------------------------------------------------------
// primeiro gráfico
subplot(3,2,1)
plot(q'*180/%pi,Xp1,q'*180/%pi,Xp2,'LineWidth',2)
xgrid
xlabel('q (^o)', 'fontsize', 3)
ylabel('X (m)', 'fontsize', 3)
title('Posição horizontal dos pontos P_1 e P_2', 'fontsize', 3)
//-------------------------------------------------------------
// segundo gráfico
subplot(3,2,2)
plot(q'*180/%pi,Yp1,q'*180/%pi,Yp2,'LineWidth',2)
xgrid
xlabel('q (^o)', 'fontsize', 3)
ylabel('Y (m)', 'fontsize', 3)
title('Posição vertical dos pontos P_1 e P_2', 'fontsize', 3)
//-------------------------------------------------------------
// terceiro gráfico
subplot(3,2,3)
plot(q'*180/%pi,Kpx1,q'*180/%pi,Kpx2,'LineWidth',2)
set(gca(),'grid',[1 1])
xlabel('q (^o)', 'fontsize', 3)
ylabel('Kpx(q)', 'fontsize', 3)
title('Coeficientes de velocidade Kpx_1 e Kpx_2', 'fontsize', 3)
//-------------------------------------------------------------
// quarto gráfico
subplot(3,2,4)
plot(q'*180/%pi,Kpy1,q'*180/%pi,Kpy2,'LineWidth',2)
xgrid
xlabel('q (^o)', 'fontsize', 3)
ylabel('Kpy(q) ', 'fontsize', 3)
title('Coeficientes de velocidade Kpy_1 e Kpy_2', 'fontsize', 3)
//-------------------------------------------------------------
// quinto gráfico
subplot(3,2,5)
plot(q'*180/%pi,Lpx1,q'*180/%pi,Lpx2,'LineWidth',2)
xgrid
xlabel('q (^o)', 'fontsize', 3)
ylabel('Lpx(q) ', 'fontsize', 3)
title('Derivada dos coeficientes de velocidade Lpx_ e Lpx_2', 'fontsize', 3)
//-------------------------------------------------------------
// sexto gráfico
subplot(3,2,6)
plot(q'*180/%pi,Lpy1,q'*180/%pi,Lpy2,'LineWidth',2)
xgrid
xlabel('q (^o)', 'fontsize', 3)
ylabel('Lpy(q) ', 'fontsize', 3)
title('Derivada dos coeficientes de velocidade Lpy_1 e Lpy_2', 'fontsize', 3)
//-------------------------------------------------------------
//// 12 - Gráfico da posição dos pontos de interesse
//-------------------------------------------------------------
scf(4)
// trajetória dos pontos de interesse
plot(Xp1,Yp1,'-',...            // trajetória do ponto P1
     Xp2,Yp2,'-',...            // trajetória do ponto P2
     'LineWidth',2)
xgrid
xlabel('X (m)', 'fontsize', 3)
ylabel('Y (m)', 'fontsize', 3)
title('Posição dos pontos de interesse', 'fontsize', 3)
legend('P_1','P_2',1);

//-------------------------------------------------------------
// FIM
//-------------------------------------------------------------








