Changed timing - now works with inverted pendulum.