working single pendulum code