Added decimal padding to make square location correct instead of always top left. Changed several parameter values. Make sure to change those back.
First full path plan for each leg that worked slightly well
FULL PATH PLAN WORKS WELL. Centered the grid on each plane at each waypoint and then made each waypoint the center of the grid