Calculate the position and angular velocity of a simple pendulum