Calculate Motion of Pendulum