Navigation updates

Navigation works better, but there is still a problem where it will turn
back around if it turns right to find a wall and doesnt see it right
away