diff --git a/Marlin/src/lcd/menu/menu_ubl.cpp b/Marlin/src/lcd/menu/menu_ubl.cpp
index c242b82fda292d2132d0efd372a827717455094b..a7fa3cf641c8a402313b4a556f214931bd262bef 100644
--- a/Marlin/src/lcd/menu/menu_ubl.cpp
+++ b/Marlin/src/lcd/menu/menu_ubl.cpp
@@ -431,9 +431,21 @@ void _lcd_ubl_map_lcd_edit_cmd() {
* UBL LCD Map Movement
*/
void ubl_map_move_to_xy() {
- current_position[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]);
- current_position[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
- planner.buffer_line(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
+ REMEMBER(fr, feedrate_mm_s, MMM_TO_MMS(XY_PROBE_SPEED));
+
+ set_destination_from_current(); // sync destination at the start
+
+ #if ENABLED(DELTA)
+ if (current_position[Z_AXIS] > delta_clip_start_height) {
+ destination[Z_AXIS] = delta_clip_start_height;
+ prepare_move_to_destination();
+ }
+ #endif
+
+ destination[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]);
+ destination[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
+
+ prepare_move_to_destination();
}
/**
@@ -461,22 +473,33 @@ void _lcd_ubl_output_map_lcd() {
if (ui.encoderPosition) {
step_scaler += (int32_t)ui.encoderPosition;
x_plot += step_scaler / (ENCODER_STEPS_PER_MENU_ITEM);
- if (ABS(step_scaler) >= ENCODER_STEPS_PER_MENU_ITEM) step_scaler = 0;
ui.encoderPosition = 0;
ui.refresh(LCDVIEW_REDRAW_NOW);
}
- // Encoder to the right (++)
- if (x_plot >= GRID_MAX_POINTS_X) { x_plot = 0; y_plot++; }
- if (y_plot >= GRID_MAX_POINTS_Y) y_plot = 0;
+ #if IS_KINEMATIC
+ #define KEEP_LOOPING true // Loop until a valid point is found
+ #else
+ #define KEEP_LOOPING false
+ #endif
+
+ do {
+ // Encoder to the right (++)
+ if (x_plot >= GRID_MAX_POINTS_X) { x_plot = 0; y_plot++; }
+ if (y_plot >= GRID_MAX_POINTS_Y) y_plot = 0;
- // Encoder to the left (--)
- if (x_plot <= GRID_MAX_POINTS_X - (GRID_MAX_POINTS_X + 1)) { x_plot = GRID_MAX_POINTS_X - 1; y_plot--; }
- if (y_plot <= GRID_MAX_POINTS_Y - (GRID_MAX_POINTS_Y + 1)) y_plot = GRID_MAX_POINTS_Y - 1;
+ // Encoder to the left (--)
+ if (x_plot < 0) { x_plot = GRID_MAX_POINTS_X - 1; y_plot--; }
+ if (y_plot < 0) y_plot = GRID_MAX_POINTS_Y - 1;
- // Prevent underrun/overrun of plot numbers
- x_plot = constrain(x_plot, GRID_MAX_POINTS_X - (GRID_MAX_POINTS_X + 1), GRID_MAX_POINTS_X + 1);
- y_plot = constrain(y_plot, GRID_MAX_POINTS_Y - (GRID_MAX_POINTS_Y + 1), GRID_MAX_POINTS_Y + 1);
+ #if IS_KINEMATIC
+ const float x = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]),
+ y = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
+ if (position_is_reachable(x, y)) break; // Found a valid point
+ x_plot += (step_scaler < 0) ? -1 : 1;
+ #endif
+
+ } while(KEEP_LOOPING);
// Determine number of points to edit
#if IS_KINEMATIC
@@ -487,6 +510,9 @@ void _lcd_ubl_output_map_lcd() {
n_edit_pts = yc ? (xc ? 9 : 6) : (xc ? 6 : 4); // Corners
#endif
+ // Cleanup
+ if (ABS(step_scaler) >= ENCODER_STEPS_PER_MENU_ITEM) step_scaler = 0;
+
if (ui.should_draw()) {
ui.ubl_plot(x_plot, y_plot);