Working on robot finding route