%========================================================================
% arquivo: BielaManivela1glSimbolico.m
% Exemplo 2.2 do Doughty
% 
% iniciado em: 25/02/2015
% prof. Jorge Luiz Erthal
%========================================================================
clear all
close all
clc
syms R L C
syms q1 A X dq1dt d2q1dt2
syms Ka Kx
%------------------------------------------------------------------------
%% vetores
S=[A; X];
q=[q1];
dqdt=[dq1dt];
d2qdt2=[d2q1dt2];
%------------------------------------------------------------------------
%% equações cinemáticas de posição
f = [R*cos(q1)+L*cos(A)-X;
     R*sin(q1)-L*sin(A)-C];
%------------------------------------------------------------------------
%% velocidades
J = jacobian(f,S);
Q = jacobian(f,q);
% K=[Ka1 Ka2; 
%    Kb1 Kb2];
K=simplify(-inv(J)*Q);
Ka=K(1,1);
Kx=K(2,1);
dSdt=K*dqdt;
dAdt=dSdt(1);
dXdt=dSdt(2);
%------------------------------------------------------------------------
%% acelerações
for i=1:size(S,1)
    for j=1:size(S,1)
        dJdq1(i,j)=jacobian(J(i,j),S)*K(:,1)+diff(J(i,j),q(1));
    end
end
for i=1:size(S,1)
    for j=1:size(q,1)
        dQdq1(i,j)=jacobian(Q(i,j),S)*K(:,1)+diff(Q(i,j),q(1));
    end
end
L1=simplify(-inv(J)*(dJdq1*K+dQdq1));
d2Sdt2=K*d2qdt2+L1*dqdt*dq1dt;
d2Adt2=d2Sdt2(1);
d2Xdt2=d2Sdt2(2);
%------------------------------------------------------------------------

%------------------------------------------------------------------------
%% dados numéricos
R=3.5;
L=10.3;
C=0.35;
q1=1.0472;
A=0.263333222;
X=11.6949279;
dq1dt=1;
d2q1dt2=1;
%------------------------------------------------------------------------
dAdtcalc=eval(dAdt);
dXdtcalc=eval(dXdt);

d2Adt2calc=eval(d2Adt2);
d2Xdt2calc=eval(d2Xdt2);
%------------------------------------------------------------------------
fprintf('dAdt = %f\n',dAdtcalc)
fprintf('dXdt = %f\n',dXdtcalc)
fprintf('d2Adt2 = %f\n',d2Adt2calc)
fprintf('d2Xdt2 = %f\n',d2Xdt2calc)