//====================================================================
// TMEC025-Mecanismos
// Prof. Jorge Luiz Erthal
// Trabalho de Mecanismos - Etapa 5
// Análise Geral de Velocidade e Aceleração
// Título do Projeto: 
// 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
//-------------------------------------------------------------
NP=37;   // Número de pontos
//-------------------------------------------------------------
// Intervalo de funcionamento
qmin = 20*%pi/180;    // valor inicial da variável primária
qmax = 65*%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];
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);
end
//-------------------------------------------------------------
// GRÁFICOS GEMINADOS DAS VARIÁVEIS SECUNDÁRIAS
scf(1)
subplot(321)                                // gráficos geminados
plot(q'*180/%pi,A*180/%pi,'r-')
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("A(q) (grau)");
title('Posição do elo intermediário','color','red','fontsize',5);
subplot(322)
plot(q'*180/%pi,B*180/%pi)
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("B(q) (grau)");
title('Posição do elo de saída','color','blue','fontsize',5);

subplot(323)                                // gráficos geminados
plot(q'*180/%pi,Ka,'r-')
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("K_a(q)");
title('Coeficiente de velocidade K_a','color','red','fontsize',5);
subplot(324)
plot(q'*180/%pi,Kb)
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("K_b(q)");
title('Coeficiente de velocidade K_b','color','blue','fontsize',5);

subplot(325)                                // gráficos geminados
plot(q'*180/%pi,La,'r-')
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("L_a(q)");
title('Derivada do coeficiente de velocidade L_a','color','red','fontsize',5);
subplot(326)
plot(q'*180/%pi,Lb)
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("L_b(q)");
title('Derivada do coeficiente de velocidade L_b','color','blue','fontsize',5);

//-------------------------------------------------------------
// GRÁFICO DO DETERMINANTE DA MATRIZ JACOBIANA
scf(2)
plot(q'*180/%pi,detJ)
set(gca(),"grid",[1 1])
xlabel("q (grau)", "fontsize", 2);
ylabel("Det J");
title('Determinante da matriz Jacobiana','color','blue','fontsize',5);
//====================================================================
//----------------------- F I M --------------------------------------
//====================================================================
