Bug Fix: Switching extruder tool change leaves Z in wrong position

The tool_change function saves the current_position to the destination
array soon after starting.  Later in the switching extruder section, the
destination array is modified when moving the Z axis up & down.  A later
section of tool_change moves the head back to the “original location”
using the destination array.  This later section assumes that the
destination array hasn’t been modified.

The fix is to save the destination Z position and then restore it after
the Z movements have completed.

Going back to using the current_position array for the switching
extruder Z axis moves (and  leaving the destination array untouched)
doesn’t fix the problem.

This bug was introduced by the “Make tool_change kinematic compatible”
commit # 847429ef which was merged on 10
Oct 2016 as part of PR 4982.

This bug was discovered in Issue 5966.