I have four first order differential equations (see attachement diff.jpg).
where
g= 9.8m/s
l1 = 0.216 m
l2 = 0.241 m
B = g/l1
alpha = l2/l1
u = 1 + m1/m2 = 2.22
// Initial values
t = 0.0;
theta1 = (141.7/180)*pi;
theta2 = (169.8/180)*pi;
p = 0.0;
q = 0.0;
the time step size , h=0.01s
please see the second order runge kutta funtion I created in C++ and advice if the method was correct
void RungeKutta(double *t, double *theta1, double *p, double *theta2, double *q, double h)
{
double tVal, theta1Val, pVal, theta2Val, qVal;
double k1, n1, k2, n2, k, n;
double K1, N1, K2, N2, K, N;
tVal = *t;
theta1Val = *theta1;
pVal = *p;
theta2Val = *theta2;
qVal = *q;
//cout << endl << tVal << " " << theta1Val << " " << pVal << " " << theta2Val << " " << qVal <<endl;
k1 = h*f(tVal, theta1Val, pVal, theta2Val, qVal);
n1 = h*g(tVal, theta1Val, pVal, theta2Val, qVal);
K1 = h*F(tVal, theta1Val, pVal, theta2Val, qVal);
N1 = h*G(tVal, theta1Val, pVal, theta2Val, qVal);
k2 = h*f(tVal+h, theta1Val+k1, pVal+n1, theta2Val+K1, qVal+N1);
n2 = h*g(tVal+h, theta1Val+k1, pVal+n1, theta2Val+K1, qVal+N1);
K2 = h*F(tVal+h, theta1Val+k1, pVal+n1, theta2Val+K1, qVal+N1);
N2 = h*G(tVal+h, theta1Val+k1, pVal+n1, theta2Val+K1, qVal+N1);
//cout << k2 << " " << n2 << " "<< K2 << " "<< N2 << " " <<endl;
k = (k1 + k2)/2;
n = (n1 + n2)/2;
K = (K1 + K2)/2;
N = (N1 + N2)/2;
// cout << endl << k << " " << n << " "<< K << " "<< N << " " <<endl;
tVal += h; theta1Val += k; pVal += n; theta2Val += K; qVal += N;
*t = tVal;
*theta1 = theta1Val;
*p = pVal;
*theta2 = theta2Val;
*q = qVal;
}