Browse Source

Improve G34, M422 (Z alignment) (#14142)

InsanityAutomation 6 years ago
parent
commit
bf64dd4db6

+ 100
- 61
Marlin/src/gcode/calibrate/G34_M422.cpp View File

@@ -73,11 +73,6 @@ void GcodeSuite::G34() {
73 73
 
74 74
   do { // break out on error
75 75
 
76
-    if (!TEST(axis_known_position, X_AXIS) || !TEST(axis_known_position, Y_AXIS)) {
77
-      SERIAL_ECHOLNPGM("Home XY first");
78
-      break;
79
-    }
80
-
81 76
     const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS);
82 77
     if (!WITHIN(z_auto_align_iterations, 1, 30)) {
83 78
       SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30).");
@@ -111,10 +106,6 @@ void GcodeSuite::G34() {
111 106
       workspace_plane = PLANE_XY;
112 107
     #endif
113 108
 
114
-    #if ENABLED(BLTOUCH)
115
-      bltouch.init();
116
-    #endif
117
-
118 109
     // Always home with tool 0 active
119 110
     #if HOTENDS > 1
120 111
       const uint8_t old_tool_index = active_extruder;
@@ -125,78 +116,126 @@ void GcodeSuite::G34() {
125 116
       extruder_duplication_enabled = false;
126 117
     #endif
127 118
 
128
-    // Before moving other axes raise Z, if needed. Never lower Z.
129
-    if (current_position[Z_AXIS] < Z_CLEARANCE_BETWEEN_PROBES) {
130
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before moving to probe pos) to ", Z_CLEARANCE_BETWEEN_PROBES);
131
-      do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
132
-    }
119
+    #if BOTH(BLTOUCH, BLTOUCH_HS_MODE)
120
+        // In BLTOUCH HS mode, the probe travels in a deployed state.
121
+        // Users of G34 might have a badly misaligned bed, so raise Z by the
122
+        // length of the deployed pin (BLTOUCH stroke < 7mm)
123
+      #define Z_BASIC_CLEARANCE Z_CLEARANCE_BETWEEN_PROBES + 7.0f
124
+    #else
125
+      #define Z_BASIC_CLEARANCE Z_CLEARANCE_BETWEEN_PROBES
126
+    #endif
127
+
128
+    // 0.05 is a 5% incline. On a 300mm bed that would be a misalignment of about 1.5cm.
129
+    // This angle is the maximum misalignment catered for
130
+    #define MAX_ANGLE 0.05f
131
+    float z_probe = Z_BASIC_CLEARANCE + MAX_ANGLE * (
132
+      #if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
133
+         SQRT(MAX(HYPOT2(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1]),
134
+                  HYPOT2(z_auto_align_xpos[1] - z_auto_align_ypos[1], z_auto_align_xpos[2] - z_auto_align_ypos[2]),
135
+                  HYPOT2(z_auto_align_xpos[2] - z_auto_align_ypos[2], z_auto_align_xpos[0] - z_auto_align_ypos[0])))
136
+      #else
137
+         HYPOT(z_auto_align_xpos[0] - z_auto_align_ypos[0], z_auto_align_xpos[1] - z_auto_align_ypos[1])
138
+      #endif
139
+    );
140
+
141
+    // Home before the alignment procedure
142
+    home_all_axes();
143
+
144
+    // Move the Z coordinate realm towards the positive - dirty trick
145
+    current_position[Z_AXIS] -= z_probe * 0.5;
133 146
 
134
-    // Remember corrections to determine errors on each iteration
135 147
     float last_z_align_move[Z_STEPPER_COUNT] = ARRAY_N(Z_STEPPER_COUNT, 10000.0f, 10000.0f, 10000.0f),
136
-          z_measured[Z_STEPPER_COUNT] = { 0 };
148
+          z_measured[Z_STEPPER_COUNT] = { 0 },
149
+          z_maxdiff = 0.0f,
150
+          amplification = z_auto_align_amplification;
151
+
152
+    uint8_t iteration;
137 153
     bool err_break = false;
138
-    for (uint8_t iteration = 0; iteration < z_auto_align_iterations; ++iteration) {
154
+    for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) {
139 155
       if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
140 156
 
141
-      // Reset minimum value
157
+      SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1));
158
+
159
+      // Initialize minimum value
142 160
       float z_measured_min = 100000.0f;
143
-      // For each iteration go through all probe positions (one per Z-Stepper)
144
-      for (uint8_t zstepper = 0; zstepper < Z_STEPPER_COUNT; ++zstepper) {
161
+      // Probe all positions (one per Z-Stepper)
162
+      for (uint8_t izstepper = 0; izstepper < Z_STEPPER_COUNT; ++izstepper) {
163
+        // iteration odd/even --> downward / upward stepper sequence 
164
+        const uint8_t zstepper = (iteration & 1) ? Z_STEPPER_COUNT - 1 - izstepper : izstepper;
145 165
 
146
-        #if BOTH(BLTOUCH, BLTOUCH_HS_MODE)
147
-          // In BLTOUCH HS mode, the probe travels in a deployed state.
148
-          // Users of G34 might have a badly misaligned bed, so raise Z by the
149
-          // length of the deployed pin (BLTOUCH stroke < 7mm)
150
-          do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES + 7);
151
-        #endif
166
+        // Safe clearance even on an incline
167
+        if (iteration == 0 || izstepper > 0) do_blocking_move_to_z(z_probe);
152 168
 
153 169
         // Probe a Z height for each stepper
154
-        z_measured[zstepper] = probe_pt(z_auto_align_xpos[zstepper], z_auto_align_ypos[zstepper], PROBE_PT_RAISE, false);
155
-
156
-        // Stop on error
157
-        if (isnan(z_measured[zstepper])) {
158
-          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> PROBING FAILED!");
170
+        if (isnan(probe_pt(z_auto_align_xpos[zstepper], z_auto_align_ypos[zstepper], PROBE_PT_RAISE, 0, true))) {
171
+          SERIAL_ECHOLNPGM("Probing failed.");
159 172
           err_break = true;
160 173
           break;
161 174
         }
162 175
 
176
+        // This is not the trigger Z value. It is the position of the probe after raising it.
177
+        // It is higher than the trigger value by a constant value (not known here). This value
178
+        // is more useful for determining the desired next iteration Z position for probing. It is
179
+        // equally well suited for determining the misalignment, just like the trigger position would be.
180
+        z_measured[zstepper] = current_position[Z_AXIS];
163 181
         if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " measured position is ", z_measured[zstepper]);
164 182
 
165
-        // Remember the maximum position to calculate the correction
183
+        // Remember the minimum measurement to calculate the correction later on
166 184
         z_measured_min = MIN(z_measured_min, z_measured[zstepper]);
167
-      }
185
+      } // for (zstepper)
168 186
 
169 187
       if (err_break) break;
170 188
 
171
-      // Remember the current z position to return to
172
-      float z_original_position = current_position[Z_AXIS];
189
+      // Adapt the next probe clearance height based on the new measurements.
190
+      // Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment.
191
+      #if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
192
+        z_maxdiff = MAX(ABS(z_measured[0] - z_measured[1]), ABS(z_measured[1] - z_measured[2]), ABS(z_measured[2] - z_measured[0]));
193
+        z_probe = Z_BASIC_CLEARANCE + MAX(z_measured[0], z_measured[1], z_measured[2]) + z_maxdiff;
194
+      #else
195
+        z_maxdiff = ABS(z_measured[0] - z_measured[1]);
196
+        z_probe = Z_BASIC_CLEARANCE + MAX(z_measured[0], z_measured[1]) + z_maxdiff;
197
+      #endif
198
+
199
+      SERIAL_ECHOPAIR("\n"
200
+        "DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1])
201
+        #if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
202
+          , " Z2-Z3=", ABS(z_measured[1] - z_measured[2])
203
+          , " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
204
+        #endif
205
+      );
206
+      SERIAL_EOL();
207
+      SERIAL_EOL();
208
+
209
+      // The following correction actions are to be enabled for select Z-steppers only
210
+      stepper.set_separate_multi_axis(true);
173 211
 
174
-      // Iterations can stop early if all corrections are below required accuracy
175 212
       bool success_break = true;
176
-      // Correct stepper offsets and re-iterate
213
+      // Correct the individual stepper offsets
177 214
       for (uint8_t zstepper = 0; zstepper < Z_STEPPER_COUNT; ++zstepper) {
178
-        stepper.set_separate_multi_axis(true);
179
-        set_all_z_lock(true); // Steppers will be enabled separately
180
-
181 215
         // Calculate current stepper move
182 216
         const float z_align_move = z_measured[zstepper] - z_measured_min,
183 217
                     z_align_abs = ABS(z_align_move);
184 218
 
185
-        // Check for lost accuracy compared to last move
219
+        // Optimize one iterations correction based on the first measurements
220
+        if (z_align_abs > 0.0f) amplification = iteration == 1 ? MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
221
+
222
+        // Check for less accuracy compared to last move
186 223
         if (last_z_align_move[zstepper] < z_align_abs - 1.0) {
187
-          // Stop here
188
-          if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> detected decreasing accuracy.");
224
+          SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
189 225
           err_break = true;
190 226
           break;
191 227
         }
192
-        else
193
-          last_z_align_move[zstepper] = z_align_abs;
194 228
 
195
-        // Only stop early if all measured points achieve accuracy target
229
+        // Remember the alignment for the next iteration
230
+        last_z_align_move[zstepper] = z_align_abs;
231
+
232
+        // Stop early if all measured points achieve accuracy target
196 233
         if (z_align_abs > z_auto_align_accuracy) success_break = false;
197 234
 
198 235
         if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " corrected by ", z_align_move);
199 236
 
237
+        // Lock all steppers except one
238
+        set_all_z_lock(true);
200 239
         switch (zstepper) {
201 240
           case 0: stepper.set_z_lock(false); break;
202 241
           case 1: stepper.set_z2_lock(false); break;
@@ -205,26 +244,25 @@ void GcodeSuite::G34() {
205 244
           #endif
206 245
         }
207 246
 
208
-        // This will lose home position and require re-homing
209
-        do_blocking_move_to_z(z_auto_align_amplification * z_align_move + current_position[Z_AXIS]);
210
-      }
247
+        // Do a move to correct part of the misalignment for the current stepper
248
+        do_blocking_move_to_z(amplification * z_align_move + current_position[Z_AXIS]);
249
+      } // for (zstepper)
250
+
251
+      // Back to normal stepper operations
252
+      set_all_z_lock(false);
253
+      stepper.set_separate_multi_axis(false);
211 254
 
212 255
       if (err_break) break;
213 256
 
214
-      // Move Z back to previous position
215
-      set_all_z_lock(true);
216
-      do_blocking_move_to_z(z_original_position);
217
-      set_all_z_lock(false);
257
+      if (success_break) { SERIAL_ECHOLNPGM("Target accuracy achieved."); break; }
218 258
 
219
-      stepper.set_separate_multi_axis(false);
259
+    } // for (iteration)
220 260
 
221
-      if (success_break) {
222
-        if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> achieved target accuracy.");
223
-        break;
224
-      }
225
-    }
261
+    if (err_break) { SERIAL_ECHOLNPGM("G34 aborted."); break; }
226 262
 
227
-    if (err_break) break;
263
+    SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations));
264
+    SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
265
+    SERIAL_EOL();
228 266
 
229 267
     // Restore the active tool after homing
230 268
     #if HOTENDS > 1
@@ -250,7 +288,8 @@ void GcodeSuite::G34() {
250 288
       bltouch._stow();
251 289
     #endif
252 290
 
253
-    gcode.G28(false);
291
+    // Home after the alignment procedure
292
+    home_all_axes();
254 293
 
255 294
   } while(0);
256 295
 

+ 31
- 42
Marlin/src/gcode/gcode.cpp View File

@@ -187,11 +187,7 @@ void GcodeSuite::dwell(millis_t time) {
187 187
 /**
188 188
  * Process the parsed command and dispatch it to its handler
189 189
  */
190
-void GcodeSuite::process_parsed_command(
191
-  #if USE_EXECUTE_COMMANDS_IMMEDIATE
192
-    const bool no_ok
193
-  #endif
194
-) {
190
+void GcodeSuite::process_parsed_command(const bool no_ok) {
195 191
   KEEPALIVE_STATE(IN_HANDLER);
196 192
 
197 193
   // Handle a known G, M, or T
@@ -802,10 +798,7 @@ void GcodeSuite::process_parsed_command(
802 798
 
803 799
   KEEPALIVE_STATE(NOT_BUSY);
804 800
 
805
-  #if USE_EXECUTE_COMMANDS_IMMEDIATE
806
-    if (!no_ok)
807
-  #endif
808
-      ok_to_send();
801
+  if (!no_ok) ok_to_send();
809 802
 }
810 803
 
811 804
 /**
@@ -831,43 +824,39 @@ void GcodeSuite::process_next_command() {
831 824
   process_parsed_command();
832 825
 }
833 826
 
834
-#if USE_EXECUTE_COMMANDS_IMMEDIATE
835
-
836
-  /**
837
-   * Run a series of commands, bypassing the command queue to allow
838
-   * G-code "macros" to be called from within other G-code handlers.
839
-   */
827
+/**
828
+ * Run a series of commands, bypassing the command queue to allow
829
+ * G-code "macros" to be called from within other G-code handlers.
830
+ */
840 831
 
841
-  void GcodeSuite::process_subcommands_now_P(PGM_P pgcode) {
842
-    char * const saved_cmd = parser.command_ptr;        // Save the parser state
843
-    for (;;) {
844
-      PGM_P const delim = strchr_P(pgcode, '\n');       // Get address of next newline
845
-      const size_t len = delim ? delim - pgcode : strlen_P(pgcode); // Get the command length
846
-      char cmd[len + 1];                                // Allocate a stack buffer
847
-      strncpy_P(cmd, pgcode, len);                      // Copy the command to the stack
848
-      cmd[len] = '\0';                                  // End with a nul
849
-      parser.parse(cmd);                                // Parse the command
850
-      process_parsed_command(true);                     // Process it
851
-      if (!delim) break;                                // Last command?
852
-      pgcode = delim + 1;                               // Get the next command
853
-    }
854
-    parser.parse(saved_cmd);                            // Restore the parser state
832
+void GcodeSuite::process_subcommands_now_P(PGM_P pgcode) {
833
+  char * const saved_cmd = parser.command_ptr;        // Save the parser state
834
+  for (;;) {
835
+    PGM_P const delim = strchr_P(pgcode, '\n');       // Get address of next newline
836
+    const size_t len = delim ? delim - pgcode : strlen_P(pgcode); // Get the command length
837
+    char cmd[len + 1];                                // Allocate a stack buffer
838
+    strncpy_P(cmd, pgcode, len);                      // Copy the command to the stack
839
+    cmd[len] = '\0';                                  // End with a nul
840
+    parser.parse(cmd);                                // Parse the command
841
+    process_parsed_command(true);                     // Process it
842
+    if (!delim) break;                                // Last command?
843
+    pgcode = delim + 1;                               // Get the next command
855 844
   }
845
+  parser.parse(saved_cmd);                            // Restore the parser state
846
+}
856 847
 
857
-  void GcodeSuite::process_subcommands_now(char * gcode) {
858
-    char * const saved_cmd = parser.command_ptr;        // Save the parser state
859
-    for (;;) {
860
-      char * const delim = strchr(gcode, '\n');         // Get address of next newline
861
-      if (delim) *delim = '\0';                         // Replace with nul
862
-      parser.parse(gcode);                              // Parse the current command
863
-      process_parsed_command(true);                     // Process it
864
-      if (!delim) break;                                // Last command?
865
-      gcode = delim + 1;                                // Get the next command
866
-    }
867
-    parser.parse(saved_cmd);                            // Restore the parser state
848
+void GcodeSuite::process_subcommands_now(char * gcode) {
849
+  char * const saved_cmd = parser.command_ptr;        // Save the parser state
850
+  for (;;) {
851
+    char * const delim = strchr(gcode, '\n');         // Get address of next newline
852
+    if (delim) *delim = '\0';                         // Replace with nul
853
+    parser.parse(gcode);                              // Parse the current command
854
+    process_parsed_command(true);                     // Process it
855
+    if (!delim) break;                                // Last command?
856
+    gcode = delim + 1;                                // Get the next command
868 857
   }
869
-
870
-#endif // USE_EXECUTE_COMMANDS_IMMEDIATE
858
+  parser.parse(saved_cmd);                            // Restore the parser state
859
+}
871 860
 
872 861
 #if ENABLED(HOST_KEEPALIVE_FEATURE)
873 862
 

+ 5
- 10
Marlin/src/gcode/gcode.h View File

@@ -312,19 +312,14 @@ public:
312 312
   static int8_t get_target_e_stepper_from_command();
313 313
   static void get_destination_from_command();
314 314
 
315
-  static void process_parsed_command(
316
-    #if USE_EXECUTE_COMMANDS_IMMEDIATE
317
-      const bool no_ok = false
318
-    #endif
319
-  );
315
+  static void process_parsed_command(const bool no_ok=false);
320 316
   static void process_next_command();
321 317
 
322
-  #if USE_EXECUTE_COMMANDS_IMMEDIATE
323
-    static void process_subcommands_now_P(PGM_P pgcode);
324
-    static void process_subcommands_now(char * gcode);
325
-  #endif
318
+  // Execute G-code as a macro, preserving parser state
319
+  static void process_subcommands_now_P(PGM_P pgcode);
320
+  static void process_subcommands_now(char * gcode);
326 321
 
327
-  FORCE_INLINE static void home_all_axes() { G28(true); }
322
+  static inline void home_all_axes() { process_subcommands_now_P(PSTR("G28")); }
328 323
 
329 324
   #if ENABLED(HOST_KEEPALIVE_FEATURE)
330 325
     /**

+ 0
- 2
Marlin/src/inc/Conditionals_post.h View File

@@ -1700,8 +1700,6 @@
1700 1700
 // If platform requires early initialization of watchdog to properly boot
1701 1701
 #define EARLY_WATCHDOG (ENABLED(USE_WATCHDOG) && defined(ARDUINO_ARCH_SAM))
1702 1702
 
1703
-#define USE_EXECUTE_COMMANDS_IMMEDIATE (ANY(G29_RETRY_AND_RECOVER, GCODE_MACROS, POWER_LOSS_RECOVERY) || HAS_DRIVER(L6470))
1704
-
1705 1703
 #if ENABLED(Z_TRIPLE_STEPPER_DRIVERS)
1706 1704
   #define Z_STEPPER_COUNT 3
1707 1705
 #elif ENABLED(Z_DUAL_STEPPER_DRIVERS)

Loading…
Cancel
Save