moveTo algorithm (for splitting moves) seems to work now. Next up... gcode re-writing!