Cleaned up controller code for diving