clear all
close all
clc
%-------------------------------------------------------------
%% dados numéricos
C1=396.30;    % Comprimento do braço inferior
C2=234.96;    % Comprimento da manga
C3=318.38;    % Comprimento do braço superior
C4= 30.00;    % Defasagem horizontal da articulaçao do br.sup
C5=210.00;    % Distancia vertical entre as articulaçoes
C6=133.04;    % Distancia da rotula superior ate a ponta de eixo
C7=100.794*pi/180;  % angulo entre a ponta de eixo e o pino mestre
C8=177.80;    % Comprimento da ponta de eixo
NP=10;
qbaixo = 40.31*pi/180;  % posiçao inferior do braço superior
qalto = -1.35*pi/180;   % posiçao superior do braço superior
%-------------------------------------------------------------
%% posição
%-------------------------------------------------------------
ef=10^(-6);   % tolerancia para o metodo de Newton Raphson 
pino0 = 10*pi/180;
binf0 = 30*pi/180;
s=[pino0; binf0];   % valor inicial usado para a primeira posiçao quando q=0
for i=1:NP
    q(i)=(i-1)*(qalto-qbaixo)/(NP-1) + qbaixo;
    f=[1;1];     %funçao f cuja norma e maior que ef
    while norm(f) > ef
        f = [C4 + C3*cos(q(i)) + C2*sin(s(1)) - C1*cos(s(2));
             C5 - C3*sin(q(i)) - C2*cos(s(1)) + C1*sin(s(2))];
        J = [C2*cos(s(1)) C1*sin(s(2));
             C2*sin(s(1)) C1*cos(s(2))];
        s=s-inv(J)*f;
    end
    % distribuição do resultado nas variáveis secundárias
    pino(i)=s(1);
    binf(i)=s(2);
    
    %% Ponto de interesse
    Up = C6-C8*cos(C7);
    Vp = C8*sin(C7);

    Ori = [C4+C3*cos(q(i));
           C5-C3*sin(q(i))];
   
    Rot = [ sin(pino(i)) cos(pino(i));
           -cos(pino(i)) sin(pino(i))];

    CoordP = Ori + Rot*[Up; Vp];
    X(i) = CoordP(1);
    Y(i) = CoordP(2);
end
camb = pino-(C7-pi/2);


varibit = max(X)-min(X)


%%
figure(1)
subplot(2,1,1)
% plot(q*180/pi,pino*180/pi);
% title('Pino Mestre')
% xlabel('Mesa superior (^o)')
% ylabel('Pino Mestre (^o)')
% axis equal
% grid on
% subplot(3,1,2)
plot(q*180/pi,binf*180/pi);
title('Mesa inferior')
xlabel('Mesa superior (^o)')
ylabel('Mesa inferior (^o)')
axis equal
grid on
subplot(2,1,2)
plot(q*180/pi,camb*180/pi);
title('Cambagem')
xlabel('Mesa superior (^o)')
ylabel('Cambagem (^o)')
axis equal
grid on

figure(2)
plot(X,Y);
title('Posicao do ponto P')
xlabel('X (mm)')
ylabel('Y (mm)')
axis equal
grid on
