V3.1 Navigation Update

-To get around the potential west wall run in problem the code was
adjusted to always straighten horizontally
-Assumptions are that the gap between the north wall is sufficient to
allow enough room that a max crooked run will not hit it