Inverted pendulum simulation

Free cart
Equations of motion
$$ \left\{ \begin{array}{ll} L\ddot\Theta = g sin\Theta + \ddot xcos\Theta \\ (m+M)\ddot x + m\ddot \Theta L cos\Theta - mL\dot \Theta^2sin\Theta = 0 \end{array} \right. $$
Numerical equations, Heun's method
$$ \left\{ \begin{array}{ll} \tilde \Theta _{i+1} = \Theta _{i+1} + hZ_i \\ \tilde Z _{i+1} = Z_i + h\frac{g}{l}sin\Theta_i \\ \Theta _{i+1} = \Theta _{i+1} + \frac{h}{2}(Z_i + \tilde Z _{i+1}) \\ Z _{i+1} = Z_i + \frac{h}{2}\frac{g}{l}(sin\Theta_i + sin\tilde \Theta _{i+1}) \\ \end{array} \right. $$
$$ \left\{ \begin{array}{ll} \tilde x_{i+1} = x_i + hY_i \\ \tilde Y_{i+1} = Y_i - h\beta sin\Theta_i(gcos\Theta_i - LZ^2_i) \\ x_{i+1} = x_i + \frac{h}{2}(Y_i + \tilde Y_{i+1}) \\ Y_{i+1} = Y_i - \frac{h}{2}\beta(sin\Theta_i(gcos\Theta_i - LZ^2_i) + sin\tilde \Theta_i(gcos\tilde \Theta_i - L\tilde Z^2_i)) \end{array} \right. $$ Where \(\beta = \frac{m}{m+M}\)