Part of path optimization done

Found first part of the path that can be optimized, and angle needed to
turn to path to the first optimized step. Need to actually turn, move
forward to just before the next step and turn to end angle.