% Funçao MatLab que determina a segunda derivada temporal de theta1 para
% uso no modelo do duplo pendulo

function D2theta1=D2theta1_calc(u)

% Propriedades do duplo pendulo
%pendulo2_data;
global m1 m2 r1 r2 L1 L2 k1 k2 c1 c2 g rf1 rf2

% Binario a aplicar
T1=u(1);

% Theta 1 e 2 e suas derivadas
theta1=u(7);
theta2=u(8);
Dtheta1=u(9);
Dtheta2=u(10);
D2theta2=u(12);

% Forças externas aplicadas em cada elo (componentes cartesianas)
Fx1=u(3);
Fy1=u(4);
Fx2=u(5);
Fy2=u(6);

% Inercia do Elo 1
J=m1*r1^2+m2*r2^2+m2*L1*(L1+2*r2*cos(theta2));

% Inercia do elo 2
J1=m2*r2*(r2+L1*cos(theta2))*D2theta2;

% Forças de Coriollis
C1=-m2*L1*r2*sin(theta2)*(Dtheta2^2+2*Dtheta1*Dtheta2);

% Forças gravitacionais
G1=g*(m1*r1+m2*L1)*sin(theta1)+m2*g*r2*sin(theta1+theta2);

% Forças de atrito
A1=c1*Dtheta1+k1*theta1;

% Forças externas
F1=Fx1*rf1*cos(theta1)+Fy1*rf1*sin(theta1)+Fx2*(L1*cos(theta1)+rf2*cos(theta1+theta2))+Fy2*(L1*sin(theta1)+rf2*sin(theta1+theta2));

% Segunda derivada de theta1
D2theta1=-1/J*(J1+C1+G1+A1-F1-T1);
