UBL on Delta's....     Should be close!    Should not affect any Cartesian printer.
This commit is contained in:
oldmcg 2017-05-11 22:33:47 -05:00 committed by Roxy-3D
parent 445003dbb8
commit 91841d75c9
10 changed files with 591 additions and 209 deletions

View file

@ -730,11 +730,16 @@
/**
* Set granular options based on the specific type of leveling
*/
#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(DELTA)
#define UBL_DELTA
#endif
#define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
#define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
#define HAS_ABL (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL))
#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || ENABLED(UBL_DELTA))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#if HAS_PROBING_PROCEDURE
#define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))
@ -779,12 +784,13 @@
#define MANUAL_PROBE_HEIGHT Z_HOMING_HEIGHT
#endif
#if IS_KINEMATIC
// Check for this in the code instead
#define MIN_PROBE_X X_MIN_POS
#define MAX_PROBE_X X_MAX_POS
#define MIN_PROBE_Y Y_MIN_POS
#define MAX_PROBE_Y Y_MAX_POS
#if ENABLED(DELTA)
// These will be further constrained in code, but UBL_PROBE_PT values
// cannot be compile-time verified within the radius.
#define MIN_PROBE_X (-DELTA_PRINTABLE_RADIUS)
#define MAX_PROBE_X ( DELTA_PRINTABLE_RADIUS)
#define MIN_PROBE_Y (-DELTA_PRINTABLE_RADIUS)
#define MAX_PROBE_Y ( DELTA_PRINTABLE_RADIUS)
#else
// Boundaries for probing based on set limits
#define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER))
@ -814,4 +820,20 @@
#define LCD_TIMEOUT_TO_STATUS 15000
#endif
/**
* DELTA_SEGMENT_MIN_LENGTH for UBL_DELTA
*/
#if ENABLED(UBL_DELTA)
#ifndef DELTA_SEGMENT_MIN_LENGTH
#if IS_SCARA
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
#elif ENABLED(DELTA)
#define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND)
#else // CARTESIAN
#define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation)
#endif
#endif
#endif
#endif // CONDITIONALS_POST_H

View file

@ -122,7 +122,7 @@
// External references
extern float feedrate;
extern float feedrate_mm_s; // must set before calling prepare_move_to_destination
extern Planner planner;
#if ENABLED(ULTRA_LCD)
extern char lcd_status_message[];
@ -130,6 +130,7 @@
extern float destination[XYZE];
void set_destination_to_current();
void set_current_to_destination();
void prepare_move_to_destination();
float code_value_float();
float code_value_linear_units();
float code_value_axis_units(const AxisEnum axis);
@ -137,9 +138,6 @@
bool code_has_value();
void lcd_init();
void lcd_setstatuspgm(const char* const message, const uint8_t level);
bool prepare_move_to_destination_cartesian();
void line_to_destination();
void line_to_destination(float);
void sync_plan_position_e();
void chirp_at_user();
@ -182,6 +180,13 @@
static int16_t g26_repeats;
void G26_line_to_destination(const float &feed_rate) {
const float save_feedrate = feedrate_mm_s;
feedrate_mm_s = feed_rate; // use specified feed rate
prepare_move_to_destination(); // will ultimately call ubl_line_to_destination_cartesian or ubl_prepare_linear_move_to for UBL_DELTA
feedrate_mm_s = save_feedrate; // restore global feed rate
}
/**
* G26: Mesh Validation Pattern generation.
*
@ -271,21 +276,10 @@
const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem
#ifdef DELTA
if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS.");
goto LEAVE;
}
#endif
// If this mesh location is outside the printable_radius, skip it.
// TODO: Change this to use `position_is_reachable`
if (!WITHIN(circle_x, X_MIN_POS, X_MAX_POS) || !WITHIN(circle_y, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to print off the bed.");
goto LEAVE;
}
if ( ! position_is_reachable_raw_xy( circle_x, circle_y ))
continue;
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index;
@ -333,9 +327,11 @@
y = circle_y + sin_table[tmp_div_30],
xe = circle_x + cos_table[tmp_div_30 + 1],
ye = circle_y + sin_table[tmp_div_30 + 1];
#ifdef DELTA
if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of
continue; // the 'circle' is on the bed. If
#if IS_KINEMATIC
// Check to make sure this segment is entirely on the bed, skip if not.
if (( ! position_is_reachable_raw_xy( x , y )) ||
( ! position_is_reachable_raw_xy( xe, ye )))
continue;
#else // not, we need to skip
x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1);
@ -463,6 +459,9 @@
sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (( position_is_reachable_raw_xy( sx, sy )) &&
( position_is_reachable_raw_xy( ex, ey ))) {
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy);
@ -474,7 +473,8 @@
}
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again
}
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it
}
}
@ -494,6 +494,9 @@
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (( position_is_reachable_raw_xy( sx, sy )) &&
( position_is_reachable_raw_xy( ex, ey ))) {
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy);
@ -504,7 +507,8 @@
debug_current_and_destination(PSTR("Connecting vertical line."));
}
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again
}
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped
}
}
}
@ -532,7 +536,7 @@
destination[Z_AXIS] = z; // We know the last_z==z or we wouldn't be in this block of code.
destination[E_AXIS] = current_position[E_AXIS];
ubl_line_to_destination(feed_value, 0);
G26_line_to_destination(feed_value);
stepper.synchronize();
set_destination_to_current();
@ -552,7 +556,7 @@
//if (ubl.g26_debug_flag) debug_current_and_destination(PSTR(" in move_to() doing last move"));
ubl_line_to_destination(feed_value, 0);
G26_line_to_destination(feed_value);
//if (ubl.g26_debug_flag) debug_current_and_destination(PSTR(" in move_to() after last move"));
@ -755,20 +759,16 @@
y_pos = current_position[Y_AXIS];
if (code_seen('X')) {
x_pos = code_value_axis_units(X_AXIS);
if (!WITHIN(x_pos, X_MIN_POS, X_MAX_POS)) {
SERIAL_PROTOCOLLNPGM("?Specified X coordinate not plausible.");
return UBL_ERR;
x_pos = code_value_float();
}
}
else
if (code_seen('Y')) {
y_pos = code_value_axis_units(Y_AXIS);
if (!WITHIN(y_pos, Y_MIN_POS, Y_MAX_POS)) {
SERIAL_PROTOCOLLNPGM("?Specified Y coordinate not plausible.");
return UBL_ERR;
y_pos = code_value_float();
}
if ( ! position_is_reachable_xy( x_pos, y_pos )) {
SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds.");
return UBL_ERR;
}
/**
@ -864,7 +864,7 @@
Total_Prime += 0.25;
if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR;
#endif
ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
stepper.synchronize(); // Without this synchronize, the purge is more consistent,
// but because the planner has a buffer, we won't be able
@ -893,7 +893,7 @@
#endif
set_destination_to_current();
destination[E_AXIS] += prime_length;
ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
stepper.synchronize();
set_destination_to_current();
retract_filament(destination);

View file

@ -429,4 +429,69 @@ void do_blocking_move_to_xy(const float &x, const float &y, const float &fr_mm_s
bool axis_unhomed_error(const bool x, const bool y, const bool z);
#endif
#endif // MARLIN_H
/**
* position_is_reachable family of functions
*/
#if IS_KINEMATIC // (DELTA or SCARA)
#if ENABLED(DELTA)
#define DELTA_PRINTABLE_RADIUS_SQUARED ((float)DELTA_PRINTABLE_RADIUS * (float)DELTA_PRINTABLE_RADIUS )
#endif
#if IS_SCARA
extern const float L1, L2;
#endif
inline bool position_is_reachable_raw_xy( float raw_x, float raw_y ) {
#if ENABLED(DELTA)
return ( HYPOT2( raw_x, raw_y ) <= DELTA_PRINTABLE_RADIUS_SQUARED );
#elif IS_SCARA
#if MIDDLE_DEAD_ZONE_R > 0
const float R2 = HYPOT2(raw_x - SCARA_OFFSET_X, raw_y - SCARA_OFFSET_Y);
return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2);
#else
return HYPOT2(raw_x - SCARA_OFFSET_X, raw_y - SCARA_OFFSET_Y) <= sq(L1 + L2);
#endif
#else // CARTESIAN
#error
#endif
}
inline bool position_is_reachable_by_probe_raw_xy( float raw_x, float raw_y ) {
// both the nozzle and the probe must be able to reach the point
return ( position_is_reachable_raw_xy( raw_x, raw_y ) &&
position_is_reachable_raw_xy(
raw_x - X_PROBE_OFFSET_FROM_EXTRUDER,
raw_y - Y_PROBE_OFFSET_FROM_EXTRUDER ));
}
#else // CARTESIAN
inline bool position_is_reachable_raw_xy( float raw_x, float raw_y ) {
// note to reviewer: this +/-0.0001 logic is copied from original postion_is_reachable
return WITHIN(raw_x, X_MIN_POS - 0.0001, X_MAX_POS + 0.0001)
&& WITHIN(raw_y, Y_MIN_POS - 0.0001, Y_MAX_POS + 0.0001);
}
inline bool position_is_reachable_by_probe_raw_xy( float raw_x, float raw_y ) {
// note to reviewer: this logic is copied from UBL_G29.cpp and does not contain the +/-0.0001 above
return WITHIN(raw_x, MIN_PROBE_X, MAX_PROBE_X)
&& WITHIN(raw_y, MIN_PROBE_Y, MAX_PROBE_Y);
}
#endif // CARTESIAN
inline bool position_is_reachable_by_probe_xy( float target_x, float target_y ) {
return position_is_reachable_by_probe_raw_xy(
RAW_X_POSITION( target_x ),
RAW_Y_POSITION( target_y ));
}
inline bool position_is_reachable_xy( float target_x, float target_y ) {
return position_is_reachable_raw_xy( RAW_X_POSITION( target_x ), RAW_Y_POSITION( target_y ));
}
#endif //MARLIN_H

View file

@ -401,7 +401,7 @@ float constexpr homing_feedrate_mm_s[] = {
#endif
MMM_TO_MMS(HOMING_FEEDRATE_Z), 0
};
static float feedrate_mm_s = MMM_TO_MMS(1500.0), saved_feedrate_mm_s;
float feedrate_mm_s = MMM_TO_MMS(1500.0), saved_feedrate_mm_s;
int feedrate_percentage = 100, saved_feedrate_percentage,
flow_percentage[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100);
@ -1677,6 +1677,8 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
#if ENABLED(DELTA)
if ( ! position_is_reachable_xy( x, y )) return;
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
set_destination_to_current(); // sync destination at the start
@ -1731,6 +1733,8 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f
#elif IS_SCARA
if ( ! position_is_reachable_xy( x, y )) return;
set_destination_to_current();
// If Z needs to raise, do it before moving XY
@ -2351,6 +2355,8 @@ static void clean_up_after_endstop_or_probe_move() {
}
#endif
if ( ! position_is_reachable_by_probe_xy( x, y )) return NAN;
const float old_feedrate_mm_s = feedrate_mm_s;
#if ENABLED(DELTA)
@ -2419,8 +2425,13 @@ static void clean_up_after_endstop_or_probe_move() {
#elif ENABLED(AUTO_BED_LEVELING_UBL)
#if ENABLED(UBL_DELTA)
if (( ubl.state.active ) && ( ! enable )) { // leveling from on to off
planner.unapply_leveling(current_position);
}
#endif
ubl.state.active = enable;
//set_current_from_steppers_for_axis(Z_AXIS);
#else
@ -3210,37 +3221,6 @@ void unknown_command_error() {
#endif // HOST_KEEPALIVE_FEATURE
bool position_is_reachable(const float target[XYZ]
#if HAS_BED_PROBE
, bool by_probe=false
#endif
) {
float dx = RAW_X_POSITION(target[X_AXIS]),
dy = RAW_Y_POSITION(target[Y_AXIS]);
#if HAS_BED_PROBE
if (by_probe) {
dx -= X_PROBE_OFFSET_FROM_EXTRUDER;
dy -= Y_PROBE_OFFSET_FROM_EXTRUDER;
}
#endif
#if IS_SCARA
#if MIDDLE_DEAD_ZONE_R > 0
const float R2 = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y);
return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2);
#else
return HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y) <= sq(L1 + L2);
#endif
#elif ENABLED(DELTA)
return HYPOT2(dx, dy) <= sq((float)(DELTA_PRINTABLE_RADIUS));
#else
const float dz = RAW_Z_POSITION(target[Z_AXIS]);
return WITHIN(dx, X_MIN_POS - 0.0001, X_MAX_POS + 0.0001)
&& WITHIN(dy, Y_MIN_POS - 0.0001, Y_MAX_POS + 0.0001)
&& WITHIN(dz, Z_MIN_POS - 0.0001, Z_MAX_POS + 0.0001);
#endif
}
/**************************************************
***************** GCode Handlers *****************
@ -3676,19 +3656,13 @@ inline void gcode_G4() {
destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT);
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
if (position_is_reachable(
destination
#if HOMING_Z_WITH_PROBE
, true
#endif
)
) {
#if HOMING_Z_WITH_PROBE
destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
#endif
if ( position_is_reachable_xy( destination[X_AXIS], destination[Y_AXIS] )) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
#endif
@ -4612,8 +4586,7 @@ void home_all_axes() { gcode_G28(); }
indexIntoAB[xCount][yCount] = abl_probe_index;
#endif
float pos[XYZ] = { xProbe, yProbe, 0 };
if (position_is_reachable(pos)) break;
if (position_is_reachable_xy( xProbe, yProbe )) break;
++abl_probe_index;
}
@ -4724,8 +4697,7 @@ void home_all_axes() { gcode_G28(); }
#if IS_KINEMATIC
// Avoid probing outside the round or hexagonal area
const float pos[XYZ] = { xProbe, yProbe, 0 };
if (!position_is_reachable(pos, true)) continue;
if (!position_is_reachable_by_probe_xy( xProbe, yProbe )) continue;
#endif
measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
@ -5028,10 +5000,9 @@ void home_all_axes() { gcode_G28(); }
*/
inline void gcode_G30() {
const float xpos = code_seen('X') ? code_value_linear_units() : current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER,
ypos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER,
pos[XYZ] = { xpos, ypos, LOGICAL_Z_POSITION(0) };
ypos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
if (!position_is_reachable(pos, true)) return;
if (!position_is_reachable_by_probe_xy( xpos, ypos )) return;
// Disable leveling so the planner won't mess with us
#if HAS_LEVELING
@ -6222,22 +6193,19 @@ inline void gcode_M42() {
bool stow_probe_after_each = code_seen('E');
float X_probe_location = code_seen('X') ? code_value_linear_units() : X_current + X_PROBE_OFFSET_FROM_EXTRUDER;
float Y_probe_location = code_seen('Y') ? code_value_linear_units() : Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER;
#if DISABLED(DELTA)
if (!WITHIN(X_probe_location, LOGICAL_X_POSITION(MIN_PROBE_X), LOGICAL_X_POSITION(MAX_PROBE_X))) {
out_of_range_error(PSTR("X"));
return;
}
#endif
float Y_probe_location = code_seen('Y') ? code_value_linear_units() : Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER;
#if DISABLED(DELTA)
if (!WITHIN(Y_probe_location, LOGICAL_Y_POSITION(MIN_PROBE_Y), LOGICAL_Y_POSITION(MAX_PROBE_Y))) {
out_of_range_error(PSTR("Y"));
return;
}
#else
float pos[XYZ] = { X_probe_location, Y_probe_location, 0 };
if (!position_is_reachable(pos, true)) {
if (!position_is_reachable_by_probe_xy(X_probe_location, Y_probe_location)) {
SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius.");
return;
}
@ -6335,7 +6303,7 @@ inline void gcode_M42() {
#else
// If we have gone out too far, we can do a simple fix and scale the numbers
// back in closer to the origin.
while (HYPOT(X_current, Y_current) > DELTA_PROBEABLE_RADIUS) {
while ( ! position_is_reachable_by_probe_xy( X_current, Y_current )) {
X_current *= 0.8;
Y_current *= 0.8;
if (verbose_level > 3) {
@ -11138,7 +11106,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#endif // AUTO_BED_LEVELING_BILINEAR
#if IS_KINEMATIC
#if IS_KINEMATIC && DISABLED(UBL_DELTA)
/**
* Prepare a linear move in a DELTA or SCARA setup.
@ -11157,6 +11125,9 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
return false;
}
// Fail if attempting move outside printable radius
if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) return true;
// Get the cartesian distances moved in XYZE
float difference[XYZE];
LOOP_XYZE(i) difference[i] = ltarget[i] - current_position[i];
@ -11245,7 +11216,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// For SCARA scale the feed rate from mm/s to degrees/s
// With segments > 1 length is 1 segment, otherwise total length
inverse_kinematics(ltarget);
ADJUST_DELTA(logical);
ADJUST_DELTA(ltarget);
const float adiff = abs(delta[A_AXIS] - oldA),
bdiff = abs(delta[B_AXIS] - oldB);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
@ -11278,7 +11249,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
else
#elif ENABLED(AUTO_BED_LEVELING_UBL)
if (ubl.state.active) {
ubl_line_to_destination(MMS_SCALED(feedrate_mm_s), active_extruder);
ubl_line_to_destination_cartesian(MMS_SCALED(feedrate_mm_s), active_extruder);
return true;
}
else
@ -11407,13 +11378,20 @@ void prepare_move_to_destination() {
#endif
#if IS_KINEMATIC
#if ENABLED(UBL_DELTA)
if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return;
#else
if (prepare_kinematic_move_to(destination)) return;
#endif
#else
#if ENABLED(DUAL_X_CARRIAGE)
if (prepare_move_to_destination_dualx()) return;
#endif
#elif ENABLED(UBL_DELTA) // will work for CARTESIAN too (smaller segments follow mesh more closely)
if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return;
#else
if (prepare_move_to_destination_cartesian()) return;
#endif
#endif
set_current_to_destination();
}
@ -12427,3 +12405,4 @@ void loop() {
endstops.report_state();
idle();
}

View file

@ -248,8 +248,9 @@
#if ENABLED(DELTA)
#if DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
#error "You probably want to use Max Endstops for DELTA!"
#elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
#error "DELTA is incompatible with ENABLE_LEVELING_FADE_HEIGHT. Please disable it."
#endif
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(UBL_DELTA)
#error "ENABLE_LEVELING_FADE_HEIGHT for DELTA requires UBL_DELTA and AUTO_BED_LEVELING_UBL."
#endif
#if ABL_GRID
#if (GRID_MAX_POINTS_X & 1) == 0 || (GRID_MAX_POINTS_Y & 1) == 0
@ -430,11 +431,20 @@ static_assert(1 >= 0
* Unified Bed Leveling
*/
#if ENABLED(AUTO_BED_LEVELING_UBL)
#if IS_KINEMATIC
#if ENABLED(DELTA)
#error "AUTO_BED_LEVELING_UBL does not yet support DELTA printers."
#elif DISABLED(NEWPANEL)
#if DISABLED(UBL_DELTA)
#error "AUTO_BED_LEVELING_UBL requires UBL_DELTA for DELTA printers."
#endif
#else // SCARA
#error "AUTO_BED_LEVELING_UBL not supported for SCARA printers."
#endif
#endif
#if DISABLED(NEWPANEL)
#error "AUTO_BED_LEVELING_UBL requires an LCD controller."
#endif
#elif ENABLED(UBL_DELTA)
#error "UBL_DELTA requires AUTO_BED_LEVELING_UBL."
#endif
/**
@ -593,11 +603,9 @@ static_assert(1 >= 0
/**
* Delta and SCARA have limited bed leveling options
*/
#if DISABLED(AUTO_BED_LEVELING_BILINEAR)
#if ENABLED(DELTA)
#error "Only AUTO_BED_LEVELING_BILINEAR is supported for DELTA bed leveling."
#elif ENABLED(SCARA)
#error "Only AUTO_BED_LEVELING_BILINEAR is supported for SCARA bed leveling."
#if IS_KINEMATIC
#if DISABLED(AUTO_BED_LEVELING_BILINEAR) && DISABLED(UBL_DELTA)
#error "Only AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL with UBL_DELTA support DELTA and SCARA bed leveling."
#endif
#endif
@ -626,6 +634,10 @@ static_assert(1 >= 0
#error "AUTO_BED_LEVELING_UBL requires EEPROM_SETTINGS. Please update your configuration."
#elif !WITHIN(GRID_MAX_POINTS_X, 3, 15) || !WITHIN(GRID_MAX_POINTS_Y, 3, 15)
#error "GRID_MAX_POINTS_[XY] must be a whole number between 3 and 15."
#endif
#if IS_CARTESIAN
#if !WITHIN(GRID_MAX_POINTS_X, 3, 15) || !WITHIN(GRID_MAX_POINTS_Y, 3, 15)
#error "GRID_MAX_POINTS_[XY] must be a whole number between 3 and 15."
#elif !WITHIN(UBL_PROBE_PT_1_X, MIN_PROBE_X, MAX_PROBE_X)
#error "The given UBL_PROBE_PT_1_X can't be reached by the Z probe."
#elif !WITHIN(UBL_PROBE_PT_2_X, MIN_PROBE_X, MAX_PROBE_X)
@ -639,6 +651,7 @@ static_assert(1 >= 0
#elif !WITHIN(UBL_PROBE_PT_3_Y, MIN_PROBE_Y, MAX_PROBE_Y)
#error "The given UBL_PROBE_PT_3_Y can't be reached by the Z probe."
#endif
#endif
#else // AUTO_BED_LEVELING_3POINT
#if !WITHIN(ABL_PROBE_PT_1_X, MIN_PROBE_X, MAX_PROBE_X)
#error "The given ABL_PROBE_PT_1_X can't be reached by the Z probe."

View file

@ -63,6 +63,7 @@
#include "temperature.h"
#include "ultralcd.h"
#include "language.h"
#include "ubl.h"
#include "Marlin.h"
@ -533,6 +534,17 @@ void Planner::check_axes_activity() {
*/
void Planner::apply_leveling(float &lx, float &ly, float &lz) {
#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA) // probably should also be enabled for UBL without UBL_DELTA
if (!ubl.state.active) return;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
if ((planner.z_fade_height) && (planner.z_fade_height <= RAW_Z_POSITION(lz))) return;
lz += ubl.state.z_offset + ( ubl.get_z_correction(lx,ly) * ubl.fade_scaling_factor_for_z(lz));
#else // no fade
lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly);
#endif // FADE
#endif // UBL
#if HAS_ABL
if (!abl_enabled) return;
#endif
@ -586,6 +598,39 @@ void Planner::check_axes_activity() {
void Planner::unapply_leveling(float logical[XYZ]) {
#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA)
if ( ubl.state.active ) {
float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]);
float z_ublmesh = ubl.get_z_correction(logical[X_AXIS],logical[Y_AXIS]);
float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height,
// Given L==U+O+M(1-U/H) (faded mesh correction formula for U<H)
// then U==L-O-M(1-U/H)
// so U==L-O-M+MU/H
// so U-MU/H==L-O-M
// so U(1-M/H)==L-O-M
// so U==(L-O-M)/(1-M/H) for U<H
if ( planner.z_fade_height ) {
float z_unfaded = z_unlevel / ( 1.0 - ( z_ublmesh * planner.inverse_z_fade_height ));
if ( z_unfaded < planner.z_fade_height ) // don't know until after compute
z_unlevel = z_unfaded;
}
#endif // ENABLE_LEVELING_FADE_HEIGHT
logical[Z_AXIS] = z_unlevel;
}
return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic
#endif
#if HAS_ABL
if (!abl_enabled) return;
#endif

View file

@ -41,7 +41,7 @@
uint8_t ubl_cnt = 0;
static void serial_echo_xy(const uint16_t x, const uint16_t y) {
static void serial_echo_xy(const int16_t x, const int16_t y) {
SERIAL_CHAR('(');
SERIAL_ECHO(x);
SERIAL_CHAR(',');

View file

@ -52,7 +52,8 @@
// ubl_motion.cpp
void debug_current_and_destination(const char * const title);
void ubl_line_to_destination(const float&, uint8_t);
void ubl_line_to_destination_cartesian(const float&, uint8_t);
bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate );
// ubl_G29.cpp
@ -329,10 +330,8 @@
* Returns 0.0 if Z is past the specified 'Fade Height'.
*/
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
inline float fade_scaling_factor_for_z(const float &lz) {
if (planner.z_fade_height == 0.0) return 1.0;
static float fade_scaling_factor = 1.0;
const float rz = RAW_Z_POSITION(lz);
if (last_specified_z != rz) {
@ -344,7 +343,10 @@
}
return fade_scaling_factor;
}
#else
inline float fade_scaling_factor_for_z(const float &lz) {
return 1.0;
}
#endif
}; // class unified_bed_leveling

View file

@ -436,8 +436,13 @@
* It may make sense to have Delta printers default to the center of the bed.
* Until that is decided, this can be forced with the X and Y parameters.
*/
x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? UBL_MESH_MAX_X : UBL_MESH_MIN_X;
y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? UBL_MESH_MAX_Y : UBL_MESH_MIN_Y;
#if IS_KINEMATIC
x_pos = X_HOME_POS;
y_pos = Y_HOME_POS;
#else // cartesian
x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS;
y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS;
#endif
}
if (code_seen('C')) {
@ -458,6 +463,11 @@
if (code_seen('H') && code_has_value()) height = code_value_float();
if ( !position_is_reachable_xy( x_pos, y_pos )) {
SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
return;
}
manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M'));
SERIAL_PROTOCOLLNPGM("G29 P2 finished.");
} break;
@ -470,17 +480,25 @@
* - Allow 'G29 P3' to choose a 'reasonable' constant.
*/
if (c_flag) {
while (repetition_cnt--) {
if ( repetition_cnt >= ( GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y )) {
for ( uint8_t x = 0; x < GRID_MAX_POINTS_X; x++ ) {
for ( uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++ ) {
ubl.z_values[x][y] = ubl_constant;
}
}
} else {
while (repetition_cnt--) { // this only populates reachable mesh points near
const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
if (location.x_index < 0) break; // No more invalid Mesh Points to populate
if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
ubl.z_values[location.x_index][location.y_index] = ubl_constant;
}
}
} else {
smart_fill_mesh(); // Do a 'Smart' fill using nearby known values
}
break;
}
else
smart_fill_mesh(); // Do a 'Smart' fill using nearby known values
} break;
case 4:
//
@ -502,6 +520,12 @@
z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
if ( isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
goto LEAVE;
}
// We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
// the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is)
@ -710,6 +734,8 @@
ubl.save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
DEPLOY_PROBE();
uint16_t max_iterations = ( GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y );
do {
if (ubl_lcd_clicked()) {
SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
@ -723,27 +749,19 @@
}
location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, do_furthest);
if (location.x_index >= 0 && location.y_index >= 0) {
if (location.x_index >= 0) { // mesh point found and is reachable by probe
const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
if (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
ubl.has_control_of_lcd_panel = false;
goto LEAVE;
}
const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level);
ubl.z_values[location.x_index][location.y_index] = measured_z;
}
if (do_ubl_mesh_map) ubl.display_map(map_type);
} while (location.x_index >= 0 && location.y_index >= 0);
LEAVE:
} while ((location.x_index >= 0) && (--max_iterations));
STOW_PROBE();
ubl.restore_ubl_active_state_and_leave();
@ -939,17 +957,13 @@
const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
ubl.has_control_of_lcd_panel = false;
goto LEAVE;
}
const float xProbe = LOGICAL_X_POSITION(rawx),
yProbe = LOGICAL_Y_POSITION(rawy);
if ( ! position_is_reachable_raw_xy( rawx, rawy )) { // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
break;
}
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
LCD_MESSAGEPGM("Moving to next");
@ -1361,13 +1375,17 @@
rawy = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
// If using the probe as the reference there are some unreachable locations.
// Also for round beds, there are grid points outside the bed that nozzle can't reach.
// Prune them from the list and ignore them till the next Phase (manual nozzle probing).
if (probe_as_reference == USE_PROBE_AS_REFERENCE &&
(!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y))
) continue;
bool reachable = probe_as_reference ?
position_is_reachable_by_probe_raw_xy( rawx, rawy ) :
position_is_reachable_raw_xy( rawx, rawy );
// Unreachable. Check if it's the closest location to the nozzle.
if ( ! reachable )
continue;
// Reachable. Check if it's the closest location to the nozzle.
// Add in a weighting factor that considers the current location of the nozzle.
const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location
@ -1415,7 +1433,13 @@
uint16_t not_done[16];
int32_t round_off;
if ( ! position_is_reachable_xy( lx, ly )) {
SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
return;
}
ubl.save_ubl_active_state_and_disable();
memset(not_done, 0xFF, sizeof(not_done));
LCD_MESSAGEPGM("Fine Tuning Mesh");
@ -1425,7 +1449,7 @@
do {
location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false);
if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points.
if (location.x_index < 0 ) break; // stop when we can't find any more reachable points.
bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a
// different location the next time through the loop
@ -1433,12 +1457,8 @@
const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility)
if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { // In theory, we don't need this check.
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to edit off the bed."); // This really can't happen, but do the check for now
ubl.has_control_of_lcd_panel = false;
goto FINE_TUNE_EXIT;
if ( ! position_is_reachable_raw_xy( rawx, rawy )) { // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable
break;
}
float new_z = ubl.z_values[location.x_index][location.y_index];
@ -1494,7 +1514,7 @@
lcd_implementation_clear();
} while (location.x_index >= 0 && location.y_index >= 0 && (--repetition_cnt>0));
} while (( location.x_index >= 0 ) && (--repetition_cnt>0));
FINE_TUNE_EXIT:

View file

@ -26,11 +26,13 @@
#include "Marlin.h"
#include "ubl.h"
#include "planner.h"
#include "stepper.h"
#include <avr/io.h>
#include <math.h>
extern float destination[XYZE];
extern void set_current_to_destination();
extern float delta_segments_per_second;
static void debug_echo_axis(const AxisEnum axis) {
if (current_position[axis] == destination[axis])
@ -87,7 +89,7 @@
}
void ubl_line_to_destination(const float &feed_rate, uint8_t extruder) {
void ubl_line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
/**
* Much of the nozzle movement will be within the same cell. So we will do as little computation
* as possible to determine if this is the case. If this move is within the same cell, we will
@ -134,7 +136,7 @@
// Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be.
planner.buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
set_current_to_destination();
if (ubl.g26_debug_flag)
@ -178,7 +180,7 @@
*/
if (isnan(z0)) z0 = 0.0;
planner.buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder);
if (ubl.g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl_line_to_destination()"));
@ -270,7 +272,7 @@
* Without this check, it is possible for the algorithm to generate a zero length move in the case
* where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
* happens, it might be best to remove the check and always 'schedule' the move because
* the planner.buffer_line() routine will filter it if that happens.
* the planner._buffer_line() routine will filter it if that happens.
*/
if (y != start[Y_AXIS]) {
if (!inf_normalized_flag) {
@ -292,7 +294,7 @@
z_position = end[Z_AXIS];
}
planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
@ -344,7 +346,7 @@
* Without this check, it is possible for the algorithm to generate a zero length move in the case
* where the line is heading left and it is starting right on a Mesh Line boundary. For how often
* that happens, it might be best to remove the check and always 'schedule' the move because
* the planner.buffer_line() routine will filter it if that happens.
* the planner._buffer_line() routine will filter it if that happens.
*/
if (x != start[X_AXIS]) {
if (!inf_normalized_flag) {
@ -363,7 +365,7 @@
z_position = end[Z_AXIS];
}
planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
@ -426,7 +428,7 @@
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
planner._buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
current_yi += dyi;
yi_cnt--;
}
@ -455,7 +457,7 @@
z_position = end[Z_AXIS];
}
planner.buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
planner._buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
current_xi += dxi;
xi_cnt--;
}
@ -472,4 +474,238 @@
set_current_to_destination();
}
#endif
#ifdef UBL_DELTA
#define COPY_XYZE( target, source ) { \
target[X_AXIS] = source[X_AXIS]; \
target[Y_AXIS] = source[Y_AXIS]; \
target[Z_AXIS] = source[Z_AXIS]; \
target[E_AXIS] = source[E_AXIS]; \
}
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor;
static float scara_oldA;
static float scara_oldB;
#endif
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
// so we call _buffer_line directly here. Per-segmented leveling performed first.
static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
#if IS_KINEMATIC
inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget
float feedrate = fr_mm_s;
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
float adiff = abs(delta[A_AXIS] - scara_oldA);
float bdiff = abs(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS];
feedrate = max(adiff, bdiff) * scara_feed_factor;
#endif
planner._buffer_line( delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder );
#else // cartesian
planner._buffer_line( ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder );
#endif
}
/**
* Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
* This calls planner._buffer_line multiple times for small incremental moves.
* Returns true if the caller did NOT update current_position, otherwise false.
*/
static bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
const float difference[XYZE] = { // cartesian distances moved in XYZE
ltarget[X_AXIS] - current_position[X_AXIS],
ltarget[Y_AXIS] - current_position[Y_AXIS],
ltarget[Z_AXIS] - current_position[Z_AXIS],
ltarget[E_AXIS] - current_position[E_AXIS]
};
float cartesian_xy_mm = sqrtf( sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) ); // total horizontal xy distance
#if IS_KINEMATIC
float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
uint16_t segments = lroundf( delta_segments_per_second * seconds ); // preferred number of segments for distance @ feedrate
uint16_t seglimit = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
NOMORE( segments, seglimit ); // limit to minimum segment length (fewer segments)
#else
uint16_t segments = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif
NOLESS( segments, 1 ); // must have at least one segment
float inv_segments = 1.0 / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = stepper.get_axis_position_degrees(A_AXIS);
scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif
const float segment_distance[XYZE] = { // length for each segment
difference[X_AXIS] * inv_segments,
difference[Y_AXIS] * inv_segments,
difference[Z_AXIS] * inv_segments,
difference[E_AXIS] * inv_segments
};
// Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore.
bool above_fade_height = false;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (( planner.z_fade_height != 0 ) &&
( planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS]) )) {
above_fade_height = true;
}
#endif
// Only compute leveling per segment if ubl active and target below z_fade_height.
if (( ! ubl.state.active ) || ( above_fade_height )) { // no mesh leveling
const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0;
float seg_dest[XYZE]; // per-segment destination,
COPY_XYZE( seg_dest, current_position ); // starting from current position
while (--segments) {
LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
float ztemp = seg_dest[Z_AXIS];
seg_dest[Z_AXIS] += z_offset;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
seg_dest[Z_AXIS] = ztemp;
}
// Since repeated adding segment_distance accumulates small errors, final move to exact destination.
COPY_XYZE( seg_dest, ltarget );
seg_dest[Z_AXIS] += z_offset;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
return false; // moved but did not set_current_to_destination();
}
// Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
float fade_scaling_factor = ubl.fade_scaling_factor_for_z(ltarget[Z_AXIS]);
#endif
float seg_dest[XYZE]; // per-segment destination, initialize to first segment
LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
const float& dx_seg = segment_distance[X_AXIS]; // alias for clarity
const float& dy_seg = segment_distance[Y_AXIS];
float rx = RAW_X_POSITION(seg_dest[X_AXIS]); // assume raw vs logical coordinates shifted but not scaled.
float ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
do { // for each mesh cell encountered during the move
// Compute mesh cell invariants that remain constant for all segments within cell.
// Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
// the bilinear interpolation from the adjacent cell within the mesh will still work.
// Inner loop will exit each time (because out of cell bounds) but will come back
// in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area.
int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
cell_xi = constrain( cell_xi, 0, (GRID_MAX_POINTS_X) - 1 );
int8_t cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
cell_yi = constrain( cell_yi, 0, (GRID_MAX_POINTS_Y) - 1 );
// float x0 = (UBL_MESH_MIN_X) + ((MESH_X_DIST) * cell_xi ); // lower left cell corner
// float y0 = (UBL_MESH_MIN_Y) + ((MESH_Y_DIST) * cell_yi ); // lower left cell corner
// float x1 = x0 + MESH_X_DIST; // upper right cell corner
// float y1 = y0 + MESH_Y_DIST; // upper right cell corner
float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi ])); // 64 byte table lookup avoids mul+add
float y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi ])); // 64 byte table lookup avoids mul+add
float x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1])); // 64 byte table lookup avoids mul+add
float y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1])); // 64 byte table lookup avoids mul+add
float cx = rx - x0; // cell-relative x
float cy = ry - y0; // cell-relative y
float z_x0y0 = ubl.z_values[cell_xi ][cell_yi ]; // z at lower left corner
float z_x1y0 = ubl.z_values[cell_xi+1][cell_yi ]; // z at upper left corner
float z_x0y1 = ubl.z_values[cell_xi ][cell_yi+1]; // z at lower right corner
float z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
if ( isnan( z_x0y0 )) z_x0y0 = 0; // ideally activating ubl.state.active (G29 A)
if ( isnan( z_x1y0 )) z_x1y0 = 0; // should refuse if any invalid mesh points
if ( isnan( z_x0y1 )) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if ( isnan( z_x1y1 )) z_x1y1 = 0; // thus guessing zero for undefined points
float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0/MESH_X_DIST); // z slope per x along y0 (lower left to lower right)
float z_xmy1 = (z_x1y1 - z_x0y1) * (1.0/MESH_X_DIST); // z slope per x along y1 (upper left to upper right)
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx
float z_cxy1 = z_x0y1 + z_xmy1 * cx; // z height along y1 at cx
float z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
float z_cxym = z_cxyd * (1.0/MESH_Y_DIST); // z slope per y along cx from y0 to y1
float z_cxcy = z_cxy0 + z_cxym * cy; // z height along cx at cy
// As subsequent segments step through this cell, the z_cxy0 intercept will change
// and the z_cxym slope will change, both as a function of cx within the cell, and
// each change by a constant for fixed segment lengths.
float z_sxy0 = z_xmy0 * dx_seg; // per-segment adjustment to z_cxy0
float z_sxym = ( z_xmy1 - z_xmy0 ) * (1.0/MESH_Y_DIST) * dx_seg; // per-segment adjustment to z_cxym
do { // for all segments within this mesh cell
z_cxcy += ubl.state.z_offset;
if ( --segments == 0 ) { // this is last segment, use ltarget for exact
COPY_XYZE( seg_dest, ltarget );
seg_dest[Z_AXIS] += z_cxcy;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
return false; // did not set_current_to_destination()
}
float z_orig = seg_dest[Z_AXIS]; // remember the pre-leveled segment z value
seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder );
seg_dest[Z_AXIS] = z_orig; // restore pre-leveled z before incrementing
LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment
cx += dx_seg;
cy += dy_seg;
if ( !WITHIN(cx,0,MESH_X_DIST) || !WITHIN(cy,0,MESH_Y_DIST)) { // done within this cell, break to next
rx = RAW_X_POSITION(seg_dest[X_AXIS]);
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
break;
}
// Next segment still within same mesh cell, adjust the per-segment
// slope and intercept and compute next z height.
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
z_cxcy = z_cxy0 + z_cxym * cy; // recompute z_cxcy from adjusted slope and intercept
} while (true); // per-segment loop exits by break after last segment within cell, or by return on final segment
} while (true); // per-cell loop
} // end of function
#endif // UBL_DELTA
#endif // AUTO_BED_LEVELING_UBL