Reverted P controller and made sure timer resets.