clear all
clc

disp('analise cinematica do mecanismo biela-manivela')
R=input('de o valor do comprimento da manivela R ');
L=input('de o valor do comprimento da biela L ');
C=input('de o valor do deslocamento do cursor em relaçao ao eixo de manivelas C ');
Up=input('de o valor da coordenada U do ponto de interesse ');
Vp=input('de o valor da coordenada V do ponto de interesse ');
N=input('de o numero de pontos para plotar graficos ');

delta_q=2*pi/N;              % intervalo de discretizaçao para a coordenada generalizada q

for i=1:N
    q(i)=(i-1)*delta_q;      % posiçoes das coordenadas generalizadas
    
    % posiçao das coordenadas secundarias
    
    A(i)=asin((R*sin(q(i))-C)/L);
    X(i)=R*cos(q(i))+L*cos(A(i));
    
    % posiçao do ponto de interese
    
    Xp(i)=R*cos(q(i))+Up*cos(A(i))+Vp*sin(A(i));
    Yp(i)=R*sin(q(i))-Up*sin(A(i))+Vp*cos(A(i));
    
    % coeficientes de velocidade das coordenadas secundarias
    
    J=[-L*sin(A(i))     -1;
       -L*cos(A(i))      0];
   
   dfdq=[-R*sin(q(i));  
          R*cos(q(i))];
      
    K=-inv(J)*dfdq;
    KA(i)=K(1);
    KX(i)=K(2);
    
    % coeficiente de velocidades do ponto de interesse
    
    Kpx(i)=-R*sin(q(i))-Up*KA(i)*sin(A(i))+Vp*KA(i)*cos(A(i));
    Kpy(i)=-R*cos(q(i))-Up*KA(i)*cos(A(i))-Vp*KA(i)*sin(A(i));
    
    % derivada de coeficiente de velocidade das coordenadas secundarias
    
    dJdq=[-L*KA(i)*cos(A(i))    0;
           L*KA(i)*sin(A(i))    0];
       
    d2fdq2=[-R*cos(q(i))   -R*sin(q(i))]';
    
    LL=-inv(J)*(dJdq*K+d2fdq2);
    LA(i)=LL(1);
    LX(i)=LL(2);
    
    % derivada de coeficiente de velocidade do ponto de interesse
    
    Lpx(i)=-R*cos(q(i))-Up*(LA(i)*sin(A(i))+KA(i)^2*cos(A(i)))+Vp*(LA(i)*cos(A(i))-KA(i)^2*sin(A(i)));
    Lpy(i)=-R*sin(q(i))-Up*(LA(i)*cos(A(i))-KA(i)^2*sin(A(i)))-Vp*(LA(i)*sin(A(i))+KA(i)^2*cos(A(i)));
end

q_graus=q*180/pi;
A_graus=A*180/pi;

figure(1)
subplot(2,1,1), plot(q_graus,A_graus)
xlabel('q em graus')
ylabel('A em gaus')
axis([0 max(q_graus) 1.1*min(A_graus) 1.1*max(A_graus)])
grid
subplot(2,1,2), plot(q_graus,X)
xlabel('q em graus')
ylabel('X')
axis([0 max(q_graus) 0.85*min(X) 1.1*max(X)])
grid

figure(2)
plot(Xp,Yp)
xlabel('Xp')
ylabel('Yp')
axis equal
title('trajetoria do ponto P')

figure(3)
subplot(2,1,1), plot(q_graus,KA)
xlabel('q em graus')
ylabel('KA')
axis([0 max(q_graus) 1.1*min(KA) 1.1*max(KA)])
grid
subplot(2,1,2), plot(q_graus,KX)
xlabel('q em graus')
ylabel('KX')
axis([0 max(q_graus) 1.1*min(KX) 1.1*max(KX)])
grid

figure(4)
subplot(2,1,1), plot(q_graus,Kpx)
xlabel('q em graus')
ylabel('Kpx')
axis([0 max(q_graus) 1.1*min(Kpx) 1.1*max(Kpx)])
grid
subplot(2,1,2), plot(q_graus,Kpy)
xlabel('q em graus')
ylabel('Kpy')
axis([0 max(q_graus) 1.1*min(Kpy) 1.1*max(Kpy)])
grid

figure(5)
subplot(2,1,1), plot(q_graus,LA)
xlabel('q em graus')
ylabel('LA')
axis([0 max(q_graus) 1.1*min(LA) 1.1*max(LA)])
grid
subplot(2,1,2), plot(q_graus,LX)
xlabel('q em graus')
ylabel('LX')
axis([0 max(q_graus) 1.1*min(LX) 1.1*max(LX)])
grid

figure(6)
subplot(2,1,1), plot(q_graus,Lpx)
xlabel('q em graus')
ylabel('Lpx')
axis([0 max(q_graus) 1.1*min(Lpx) 1.1*max(Lpx)])
grid
subplot(2,1,2), plot(q_graus,Lpy)
xlabel('q em graus')
ylabel('Lpy')
axis([0 max(q_graus) 1.1*min(Lpy) 1.1*max(Lpy)])
grid