Cleaned up code, yaw and pitch now smooth and awesome