%====================================================================
% TMEC025-Mecanismos - ANO/SEMESTRE
% Prof. Jorge Luiz Erthal
% Trabalho de Mecanismos - Etapa 5
% Análise Geral de Velocidade e Aceleração
% Título do Projeto: Mecanismo de 4 barras
% Equipe no.:
% Componentes da equipe:
% 1- 
% 2- 
% 3-
% 4-
%====================================================================
clear variables
close all
clc
%-------------------------------------------------------------
%% 1 - Dados numéricos
%-------------------------------------------------------------
C1=6;        
C2=5;
C3=2;
C4=4;
%-------------------------------------------------------------
% Número de pontos
%-------------------------------------------------------------
NP = 37;
%-------------------------------------------------------------
% Alocação de memória
A=zeros(NP);
B=zeros(NP);
detJ=zeros(NP);
Ka=zeros(NP);
Kb=zeros(NP);
La=zeros(NP);
Lb=zeros(NP);
%-------------------------------------------------------------
% Intervalo de funcionamento
%-------------------------------------------------------------
qmin = 20*pi/180;
qmax = 65*pi/180;
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 iniciais das variaveis secundarias
a = 90*pi/180;
b = 100*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 strcmp(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-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
%-------------------------------------------------------------
%% 8 - Gráficos da análise geral
%-------------------------------------------------------------
figure('Name','Análise Geral')    % título na moldura superior
% gráficos geminados
%-------------------------------------------------------------
% gráfico 1
subplot(3,2,1)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,A*180/pi,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('A(q) (^o)')               % rótulo do eixo y
title('Posição da barra C_2')         % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% gráfico 2
subplot(3,2,2)                    % gráfico da direita (posição 2)
plot(q*180/pi,B*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('B(q) (^o)')               % rótulo do eixo y
title('Posição da barra C_3')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% gráfico 3
subplot(3,2,3)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,Ka,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('K_{a}(q) ')                  % rótulo do eixo y
title('Coeficiente de velocidade K_{a}')       % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% gráfico 4
subplot(3,2,4)                    % gráfico da direita (posição 2)
plot(q*180/pi,Kb,...   
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('K_{b}(q) ')                  % rótulo do eixo y
title('Coeficiente de velocidade K_{b}')       % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% gráfico 5
subplot(3,2,5)                    % gráfico da esquerda (posição 1) 
plot(q*180/pi,La,...                       
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('L_{a}(q) ')                  % rótulo do eixo y
title('Derivada do coeficiente de velocidade L_{a}')    % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
% gráfico 6
subplot(3,2,6)                    % gráfico da direita (posição 2)
plot(q*180/pi,Lb,... 
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('L_{b}(q) ')                  % rótulo do eixo y
title('Derivada do coeficiente de velocidade L_{b}')    % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
%% 9 - Gráfico do determinante de J
%-------------------------------------------------------------
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])

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