Prevent a tool change move out of bounds

Marlin/src/module/tool_change.cpp

 
 #endif // SWITCHING_NOZZLE
 
+inline void fast_line_to_current(const AxisEnum fr_axis) {
+  planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[fr_axis], active_extruder);
+}
+
 #if ENABLED(MAGNETIC_PARKING_EXTRUDER)
 
   float parkingposx[2] ,           // M951 R L
         if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
       #endif
 
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Z_AXIS], active_extruder);
+      fast_line_to_current(Z_AXIS);
       planner.synchronize();
 
       // STEP 2
         }
       #endif
 
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+      fast_line_to_current(X_AXIS);
       planner.synchronize();
 
       // STEP 3
         if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position);
       #endif
 
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+      fast_line_to_current(X_AXIS);
       planner.synchronize();
 
       // STEP 5
       // STEP 6
 
       current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10);
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+      fast_line_to_current(X_AXIS);
       current_position[X_AXIS] = grabpos;
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Unpark extruder", current_position);
       #endif
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]/2, active_extruder);
+      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS] * 0.5, active_extruder);
       planner.synchronize();
 
       // STEP 7
         if (DEBUGGING(LEVELING)) DEBUG_POS("(7) Move midway between hotends", current_position);
       #endif
 
-      planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+      fast_line_to_current(X_AXIS);
       planner.synchronize();
 
       #if ENABLED(DEBUG_LEVELING_FEATURE)
      * 6. Apply the z-offset of the new toolhead
      */
 
-    // STEP 1
+    // 1. Raise Z to give enough clearance
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) DEBUG_POS("Starting Toolhead change", current_position);
       if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Z_AXIS], active_extruder);
+    fast_line_to_current(Z_AXIS);
     planner.synchronize();
 
-    // STEP 2
+    // 2. Move to switch position of current toolhead
 
     current_position[X_AXIS] = placexpos;
 
       }
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+    fast_line_to_current(X_AXIS);
     planner.synchronize();
 
     current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
       if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS], active_extruder);
+    fast_line_to_current(Y_AXIS);
     planner.synchronize();
 
-    // STEP 3
+    // 3. Unlock tool and drop it in the dock
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("(3) Unlock and Place Toolhead");
       if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
+    fast_line_to_current(Y_AXIS); // move away from docked toolhead
     planner.synchronize();
 
-    // STEP 4
+    // 4. Move to the new toolhead
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("(4) Move to new toolhead position");
       if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
+    fast_line_to_current(X_AXIS);
     planner.synchronize();
     current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
 
       if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS], active_extruder);
+    fast_line_to_current(Y_AXIS);
     planner.synchronize();
 
-    // STEP 5
+    // 5. Grab and lock the new toolhead
 
     #if ENABLED(DEBUG_LEVELING_FEATURE)
       if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("(5) Grab and lock new toolhead ");
       if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
     #endif
 
-    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
+    fast_line_to_current(Y_AXIS); // move away from docked toolhead
     planner.synchronize();
 
-    // STEP 6
+    // 6. Apply the z-offset of the new toolhead
 
     #if HAS_HOTEND_OFFSET
       current_position[Z_AXIS] += hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder];
     ) {
 
       #if ENABLED(DEBUG_LEVELING_FEATURE)
-        if (DEBUGGING(LEVELING)) {
-          SERIAL_ECHOLNPAIR("MoveX to ", xhome);
-        }
+        if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("MoveX to ", xhome);
       #endif
       // Park old head
       planner.buffer_line(xhome, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], planner.settings.max_feedrate_mm_s[X_AXIS], active_extruder);
         #if HAS_SOFTWARE_ENDSTOPS
           NOMORE(current_position[Z_AXIS], soft_endstop_max[Z_AXIS]);
         #endif
-        if (!no_move)planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Z_AXIS], active_extruder);
+        if (!no_move) fast_line_to_current(Z_AXIS);
         move_nozzle_servo(tmp_extruder);
       #endif
 
           }
         #endif
 
+        // Prevent a move outside physical bounds
+        clamp_to_software_endstops(destination);
+
         // Move back to the original (or tweaked) position
         do_blocking_move_to(destination);
+
         #if ENABLED(DUAL_X_CARRIAGE)
           active_extruder_parked = false;
         #endif