Fix nullpointer and infinite loop that the robot could get stuck in if the pi is not connected