Better code, still need to work on pitch and yaw