Add'l toolchange syncs

Marlin/src/module/tool_change.cpp

@@ -135,11 +135,11 @@ inline void fast_line_to_current(const AxisEnum fr_axis) {
 
 #if ENABLED(MAGNETIC_PARKING_EXTRUDER)
 
-  float parkingposx[2] ,           // M951 R L
-        parkinggrabdistance ,      // M951 I
-        parkingslowspeed,          // M951 J
-        parkinghighspeed ,         // M951 H
-        parkingtraveldistance,     // M951 D
+  float parkingposx[2],           // M951 R L
+        parkinggrabdistance,      // M951 I
+        parkingslowspeed,         // M951 J
+        parkinghighspeed,         // M951 H
+        parkingtraveldistance,    // M951 D
         compensationmultiplier;
 
   inline void magnetic_parking_extruder_tool_change(const uint8_t tmp_extruder) {
@@ -290,52 +290,50 @@ inline void fast_line_to_current(const AxisEnum fr_axis) {
 
       // STEP 1
 
-      if (DEBUGGING(LEVELING)) DEBUG_POS("Start Autopark", current_position);
+      if (DEBUGGING(LEVELING)) DEBUG_POS("Start PE Tool-Change", current_position);
 
       current_position[X_AXIS] = parkingposx[active_extruder] + x_offset;
-
       if (DEBUGGING(LEVELING)) {
         DEBUG_ECHOLNPAIR("(1) Park extruder ", int(active_extruder));
         DEBUG_POS("Moving ParkPos", current_position);
       }
-
       fast_line_to_current(X_AXIS);
-      planner.synchronize();
 
       // STEP 2
 
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(2) Disengage magnet ");
-
+      planner.synchronize();
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(2) Disengage magnet");
       pe_deactivate_solenoid(active_extruder);
 
       // STEP 3
 
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(3) Move near new extruder");
-
       current_position[X_AXIS] += active_extruder ? -10 : 10; // move 10mm away from parked extruder
-
-      if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position);
-
+      if (DEBUGGING(LEVELING)) {
+        DEBUG_ECHOLNPGM("(3) Move near new extruder");
+        DEBUG_POS("Move away from parked extruder", current_position);
+      }
       fast_line_to_current(X_AXIS);
-      planner.synchronize();
 
       // STEP 4
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(4) Engage magnetic field");
 
+      planner.synchronize();
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(4) Engage magnetic field");
       #if ENABLED(PARKING_EXTRUDER_SOLENOIDS_INVERT)
-        pe_activate_solenoid(active_extruder); //just save power for inverted magnets
+        pe_activate_solenoid(active_extruder); // Just save power for inverted magnets
       #endif
-
       pe_activate_solenoid(tmp_extruder);
 
       // STEP 5
 
       current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10);
       fast_line_to_current(X_AXIS);
+
       current_position[X_AXIS] = grabpos;
-      if (DEBUGGING(LEVELING)) DEBUG_POS("(5) Unpark extruder", current_position);
+      if (DEBUGGING(LEVELING)) {
+        planner.synchronize();
+        DEBUG_POS("(5) Unpark extruder", current_position);
+      }
       planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS] * 0.5, active_extruder);
-      planner.synchronize();
 
       // STEP 6
 
@@ -344,13 +342,14 @@ inline void fast_line_to_current(const AxisEnum fr_axis) {
           - hotend_offset[X_AXIS][tmp_extruder]
         #endif
       ;
-
-      if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Move midway between hotends", current_position);
-
+      if (DEBUGGING(LEVELING)) {
+        planner.synchronize();
+        DEBUG_POS("(6) Move midway between hotends", current_position);
+      }
       fast_line_to_current(X_AXIS);
-      planner.synchronize();
+      planner.synchronize(); // Always sync the final move
 
-      DEBUG_ECHOLNPGM("Autopark done.");
+      DEBUG_ECHOLNPGM("PE Tool-Change done.");
     }
     else { // nomove == true
       // Only engage magnetic field for new extruder
@@ -383,86 +382,80 @@ inline void fast_line_to_current(const AxisEnum fr_axis) {
 
     // 1. Move to switch position of current toolhead
 
-    if (DEBUGGING(LEVELING)) DEBUG_POS("Starting Toolhead change", current_position);
+    if (DEBUGGING(LEVELING)) DEBUG_POS("Start ST Tool-Change", current_position);
 
     current_position[X_AXIS] = placexpos;
-
     if (DEBUGGING(LEVELING)) {
       DEBUG_ECHOLNPAIR("(1) Place old tool ", int(active_extruder));
       DEBUG_POS("Move X SwitchPos", current_position);
     }
-
     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);
 
+    current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
+    if (DEBUGGING(LEVELING)) {
+      planner.synchronize();
+      DEBUG_POS("Move Y SwitchPos + Security", current_position);
+    }
     fast_line_to_current(Y_AXIS);
-    planner.synchronize();
 
     // 2. Unlock tool and drop it in the dock
 
+    planner.synchronize();
     if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(2) Unlock and Place Toolhead");
-
     MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[1]);
     safe_delay(500);
 
     current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
-
     if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
+    planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.5f, active_extruder);
 
-    planner.buffer_line(current_position,(planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
+    // Wait for move to complete, then another 0.2s
     planner.synchronize();
     safe_delay(200);
-    current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
 
+    current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
     if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
-
     fast_line_to_current(Y_AXIS); // move away from docked toolhead
-    planner.synchronize();
 
     // 3. Move to the new toolhead
 
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
-
     current_position[X_AXIS] = grabxpos;
-
-    if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
-
+    if (DEBUGGING(LEVELING)) {
+      planner.synchronize();
+      DEBUG_ECHOLNPGM("(3) Move to new toolhead position");
+      DEBUG_POS("Move to new toolhead X", current_position);
+    }
     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);
 
+    current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - (SWITCHING_TOOLHEAD_Y_SECURITY);
+    if (DEBUGGING(LEVELING)) {
+      planner.synchronize();
+      DEBUG_POS("Move Y SwitchPos + Security", current_position);
+    }
     fast_line_to_current(Y_AXIS);
-    planner.synchronize();
 
     // 4. Grab and lock the new toolhead
 
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(4) Grab and lock new toolhead ");
-
     current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
-
-    if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
-
+    if (DEBUGGING(LEVELING)) {
+      planner.synchronize();
+      DEBUG_ECHOLNPGM("(4) Grab and lock new toolhead");
+      DEBUG_POS("Move Y SwitchPos", current_position);
+    }
     planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
-    planner.synchronize();
 
+    // Wait for move to finish, pause 0.2s, move servo, pause 0.5s
+    planner.synchronize();
     safe_delay(200);
     MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[0]);
     safe_delay(500);
 
     current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
-
     if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
+    fast_line_to_current(Y_AXIS); // Move away from docked toolhead
+    planner.synchronize();        // Always sync the final move
 
-    fast_line_to_current(Y_AXIS); // move away from docked toolhead
-    planner.synchronize();
-
-    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Toolhead change done.");
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("ST Tool-Change done.");
   }
 
 #elif ENABLED(MAGNETIC_SWITCHING_TOOLHEAD)