How to make a simulation of double pendulum using runge-kutta 4th order

Hello everyone, I need your valuable help here.

I was writing a code for simulating the motion of double pendulum using runge-kutta 4th order method but it doesn't work well.

Could you help me to check my code and tell me where did I go wrong?

clc
clear all
close all

m1      = 1;
m2      = 1;
L1      = 1;
L2      = 1;
g       = 9.8;
h       = 0.1;

theta1  = 3*pi/180;
theta2  = 0;
w1      = 0;
w2      = 0;

time   = [0,10];
n       = ceil((diff(time))/h);
t       = linspace(time(1),time(2),n);

x1(1) = 0;
y1(1) = 0;

for i = 2:n
    p1      = w11(w1);
    q1      = w22(w2);
    r1      = t11(theta1, theta2, w1, w2, m1, m2, L1, L2, g);
    s1      = t22(theta1, theta2, w1, w2, m1, m2, L1, L2, g);
    
    p2      = h*(w11(w1+(r1/2)));
    q2      = h*(w22(w2+(s1/2)));
    r2      = h*(t11((theta1+(p1/2.)), (theta2+(q1/2.)), (w1+(r1/2.)), (w2+(s1/2.)),...
              m1, m2, L1, L2, g));
    s2      = h*(t22((theta1+(p1/2.)), (theta2+(q1/2.)), (w1+(r1/2.)), (w2+(s1/2.)),...
              m1, m2, L1, L2, g));
      
    p3      = h*w11(w1+(r2/2.));
    q3      = h*w22(w2+(s2/2.));
    r3      = h*(t11((theta1+(p2/2.)), (theta2+(q2/2.)), (w1+(r2/2.)), (w2+(s2/2.)),...
              m1, m2, L1, L2, g));
    s3      = h*(t22((theta1+(p2/2.)), (theta2+(q2/2.)), (w1+(r2/2.)), (w2+(s2/2.)),...
              m1, m2, L1, L2, g));
    
    p4      = h*w11(w1+(r3));
    q4      = h*w22(w2+(s3));
    r4      = h*(t11((theta1+(p3)), (theta2+(q3)), (w1+(r3)), (w2+(s3)), m1, m2,...
              L1, L2, g));
    s4      = h*(t22((theta1+(p3)), (theta2+(q3)), (w1+(r3)), (w2+(s3)), m1, m2,...
              L1, L2, g));
    
   theta1   = theta1 + (1/6)*(p1+(2.*p2)+(2.*p3)+p4);
   theta2   = theta2 + (1/6)*(q1+(2.*q2)+(2.*q3)+q4);
    T1(:,i) = theta1;
    T2(:,i) = theta2;
    
end

figure();

for i = 1:length(t);
    
    x1(2)   = L1*sin(T1(1,i));
    y1(2)   = (-L1)*cos(T1(1,i));
    x2(1)   = x1(2);
    x2(2)   = x1(2)+L2*sin(T2(1,i));
    y2(1)   = y1(2);
    y2(2)   = y1(2)-L2*cos(T2(1,i));
   
    plot(x1, y1,'k-');
    hold on
    plot (x1(2),y1(2), 'k.','markersize',25);
    plot (x2, y2, 'k-');
    plot (x2(2),y2(2), 'k.','markersize',25);
    hold off
    axis equal
    axis([-3 3 -3 3]);
    title([num2str(t(i)),'sekon']) 
    pause(h)
    
end
%% here is the function i used
function t1 = t11(theta1, theta2, w1, w2, m1, m2, L1, L2, g,t)

a       = (m1+m2)*L1;
b       = m2.*L2.*cos(theta1-theta2);
c       = m2.*L1.*cos(theta1-theta2);
d       = m2.*L2;
e       = (-m2.*L2.*w2.*w2.*sin(theta1-theta2))-(g.*(m1+m2).*sin(theta1));
f       = m2.*L1.*w1.*w1.*sin(theta1-theta2)-m2.*g.*sin(theta1);

t1      = ((e.*f-f.*b)/(a.*d-b.*c));
end

function t2 = t22(theta1, theta2, w1, w2, m1, m2, L1, L2, g,t)

a       = (m1+m2)*L1;
b       = m2.*L2.*cos(theta1-theta2);
c       = m2.*L1.*cos(theta1-theta2);
d       = m2.*L2;
e       = -m2.*L2.*w2.*w2.*sin(theta1-theta2)-g.*(m1+m2).*sin(theta1);
f       = m2.*L1.*w1.*w1.*sin(theta1-theta2)-m2.*g.*sin(theta1);

t2      = (c.*e-a.*f)/(b.*c-a.*d);
end

function W1 = w11(w1,i)
W1      = w1;

function W2 = w22(w2,i)
W2      = w2;

Answer 1

[KSSV]

https://in.mathworks.com/matlabcentral/fileexchange/27212-animated-double-pendulum

Answer 2

[madhan ravi]

Just download the file exchange below and adapt it to your needs:

https://www.mathworks.com/matlabcentral/fileexchange/29851-runge-kutta-4th-order-ode