CMS/Marlin
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleanups to UBL_DELTA

Browse Source
This commit is contained in:
Scott Lahteine 2017-05-12 01:05:11 -05:00
parent cbfca29522
commit 0696dda470
6 changed files with 116 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Marlin/Conditionals_post.h
View File

@ -731,15 +731,12 @@
* Set granular options based on the specific type of leveling * Set granular options based on the specific type of leveling
*/ */
#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(DELTA) #define UBL_DELTA (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_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 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_ABL (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL))
#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || ENABLED(UBL_DELTA)) #define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || UBL_DELTA)
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#if HAS_PROBING_PROCEDURE #if HAS_PROBING_PROCEDURE
#define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION)) #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))
@ -823,8 +820,7 @@
/** /**
* DELTA_SEGMENT_MIN_LENGTH for UBL_DELTA * DELTA_SEGMENT_MIN_LENGTH for UBL_DELTA
*/ */
#if UBL_DELTA
#if ENABLED(UBL_DELTA)
#ifndef DELTA_SEGMENT_MIN_LENGTH #ifndef DELTA_SEGMENT_MIN_LENGTH
#if IS_SCARA #if IS_SCARA
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm

13
Marlin/G26_Mesh_Validation_Tool.cpp
View File

@ -278,8 +278,7 @@
// If this mesh location is outside the printable_radius, skip it. // If this mesh location is outside the printable_radius, skip it.
if ( ! position_is_reachable_raw_xy( circle_x, circle_y )) if (!position_is_reachable_raw_xy(circle_x, circle_y)) continue;
continue;
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index; yi = location.y_index;
@ -329,9 +328,7 @@
ye = circle_y + sin_table[tmp_div_30 + 1]; ye = circle_y + sin_table[tmp_div_30 + 1];
#if IS_KINEMATIC #if IS_KINEMATIC
// Check to make sure this segment is entirely on the bed, skip if not. // Check to make sure this segment is entirely on the bed, skip if not.
if (( ! position_is_reachable_raw_xy( x , y )) || if (!position_is_reachable_raw_xy(x, y) || !position_is_reachable_raw_xy(xe, ye)) continue;
( ! position_is_reachable_raw_xy( xe, ye )))
continue;
#else // not, we need to skip #else // not, we need to skip
x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops 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); y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1);
@ -459,8 +456,7 @@
sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1); 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); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (( position_is_reachable_raw_xy( sx, sy )) && if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
( position_is_reachable_raw_xy( ex, ey ))) {
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
@ -494,8 +490,7 @@
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = constrain(ey, 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 )) && if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
( position_is_reachable_raw_xy( ex, ey ))) {
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);

43
Marlin/Marlin_main.cpp
View File

@ -2427,9 +2427,12 @@ static void clean_up_after_endstop_or_probe_move() {
#elif ENABLED(AUTO_BED_LEVELING_UBL) #elif ENABLED(AUTO_BED_LEVELING_UBL)
#if ENABLED(UBL_DELTA) #if PLANNER_LEVELING
if (( ubl.state.active ) && ( ! enable )) { // leveling from on to off if (ubl.state.active != enable) {
planner.unapply_leveling(current_position); if (!enable) // leveling from on to off
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
else
planner.unapply_leveling(current_position);
} }
#endif #endif
@ -11104,7 +11107,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#endif // AUTO_BED_LEVELING_BILINEAR #endif // AUTO_BED_LEVELING_BILINEAR
#if IS_KINEMATIC && DISABLED(UBL_DELTA) #if IS_KINEMATIC && !UBL_DELTA
/** /**
* Prepare a linear move in a DELTA or SCARA setup. * Prepare a linear move in a DELTA or SCARA setup.
@ -11124,7 +11127,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
} }
// Fail if attempting move outside printable radius // Fail if attempting move outside printable radius
if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) return true; if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true;
// Get the cartesian distances moved in XYZE // Get the cartesian distances moved in XYZE
float difference[XYZE]; float difference[XYZE];
@ -11225,7 +11228,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
return false; return false;
} }
#else // !IS_KINEMATIC #else // !IS_KINEMATIC || UBL_DELTA
/** /**
* Prepare a linear move in a Cartesian setup. * Prepare a linear move in a Cartesian setup.
@ -11263,7 +11266,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
return false; return false;
} }
#endif // !IS_KINEMATIC #endif // !IS_KINEMATIC || UBL_DELTA
#if ENABLED(DUAL_X_CARRIAGE) #if ENABLED(DUAL_X_CARRIAGE)
@ -11375,21 +11378,21 @@ void prepare_move_to_destination() {
#endif #endif
#if IS_KINEMATIC if (
#if ENABLED(UBL_DELTA) #if IS_KINEMATIC
if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return; #if UBL_DELTA
ubl_prepare_linear_move_to(destination, feedrate_mm_s)
#else
prepare_kinematic_move_to(destination)
#endif
#elif ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx()
#elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
ubl_prepare_linear_move_to(destination, feedrate_mm_s)
#else #else
if (prepare_kinematic_move_to(destination)) return; prepare_move_to_destination_cartesian()
#endif #endif
#else ) return;
#if ENABLED(DUAL_X_CARRIAGE)
if (prepare_move_to_destination_dualx()) return;
#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(); set_current_to_destination();
} }

29
Marlin/SanityCheck.h
View File

@ -248,11 +248,9 @@
#if ENABLED(DELTA) #if ENABLED(DELTA)
#if DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG) #if DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
#error "You probably want to use Max Endstops for DELTA!" #error "You probably want to use Max Endstops for DELTA!"
#endif #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(UBL_DELTA) #error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL."
#error "ENABLE_LEVELING_FADE_HEIGHT for DELTA requires UBL_DELTA and AUTO_BED_LEVELING_UBL." #elif ABL_GRID
#endif
#if ABL_GRID
#if (GRID_MAX_POINTS_X & 1) == 0 || (GRID_MAX_POINTS_Y & 1) == 0 #if (GRID_MAX_POINTS_X & 1) == 0 || (GRID_MAX_POINTS_Y & 1) == 0
#error "DELTA requires GRID_MAX_POINTS_X and GRID_MAX_POINTS_Y to be odd numbers." #error "DELTA requires GRID_MAX_POINTS_X and GRID_MAX_POINTS_Y to be odd numbers."
#elif GRID_MAX_POINTS_X < 3 #elif GRID_MAX_POINTS_X < 3
@ -431,20 +429,11 @@ static_assert(1 >= 0
* Unified Bed Leveling * Unified Bed Leveling
*/ */
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#if IS_KINEMATIC #if IS_SCARA
#if ENABLED(DELTA) #error "AUTO_BED_LEVELING_UBL does not yet support SCARA printers."
#if DISABLED(UBL_DELTA) #elif DISABLED(NEWPANEL)
#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." #error "AUTO_BED_LEVELING_UBL requires an LCD controller."
#endif #endif
#elif ENABLED(UBL_DELTA)
#error "UBL_DELTA requires AUTO_BED_LEVELING_UBL."
#endif #endif
/** /**
@ -603,10 +592,8 @@ static_assert(1 >= 0
/** /**
* Delta and SCARA have limited bed leveling options * Delta and SCARA have limited bed leveling options
*/ */
#if IS_KINEMATIC #if IS_SCARA && DISABLED(AUTO_BED_LEVELING_BILINEAR)
#if DISABLED(AUTO_BED_LEVELING_BILINEAR) && DISABLED(UBL_DELTA) #error "Only AUTO_BED_LEVELING_BILINEAR currently supports SCARA bed leveling."
#error "Only AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL with UBL_DELTA support DELTA and SCARA bed leveling."
#endif
#endif #endif
/** /**

20
Marlin/planner.cpp
View File

@ -534,12 +534,12 @@ void Planner::check_axes_activity() {
*/ */
void Planner::apply_leveling(float &lx, float &ly, float &lz) { 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 ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA // probably should also be enabled for UBL without UBL_DELTA
if (!ubl.state.active) return; if (!ubl.state.active) return;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// if z_fade_height enabled (nonzero) and raw_z above it, no leveling required // 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; 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)); lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly) * ubl.fade_scaling_factor_for_z(lz);
#else // no fade #else // no fade
lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly); lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly);
#endif // FADE #endif // FADE
@ -598,13 +598,13 @@ void Planner::check_axes_activity() {
void Planner::unapply_leveling(float logical[XYZ]) { void Planner::unapply_leveling(float logical[XYZ]) {
#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA) #if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
if ( ubl.state.active ) { if (ubl.state.active) {
float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]); const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
float z_ublmesh = ubl.get_z_correction(logical[X_AXIS],logical[Y_AXIS]); z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh; float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
@ -616,9 +616,9 @@ void Planner::check_axes_activity() {
// so U(1-M/H)==L-O-M // so U(1-M/H)==L-O-M
// so U==(L-O-M)/(1-M/H) for U<H // so U==(L-O-M)/(1-M/H) for U<H
if ( planner.z_fade_height ) { if (planner.z_fade_height) {
float z_unfaded = z_unlevel / ( 1.0 - ( z_ublmesh * planner.inverse_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 if (z_unfaded < planner.z_fade_height) // don't know until after compute
z_unlevel = z_unfaded; z_unlevel = z_unfaded;
} }

149
Marlin/ubl_motion.cpp
View File

@ -474,20 +474,10 @@
set_current_to_destination(); set_current_to_destination();
} }
#if UBL_DELTA
#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 #if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor; static float scara_feed_factor, scara_oldA, scara_oldB;
static float scara_oldA;
static float scara_oldB;
#endif #endif
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
@ -501,18 +491,18 @@
float feedrate = fr_mm_s; float feedrate = fr_mm_s;
#if IS_SCARA // scale the feed rate from mm/s to degrees/s #if IS_SCARA // scale the feed rate from mm/s to degrees/s
float adiff = abs(delta[A_AXIS] - scara_oldA); float adiff = abs(delta[A_AXIS] - scara_oldA),
float bdiff = abs(delta[B_AXIS] - scara_oldB); bdiff = abs(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS]; scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS]; scara_oldB = delta[B_AXIS];
feedrate = max(adiff, bdiff) * scara_feed_factor; feedrate = max(adiff, bdiff) * scara_feed_factor;
#endif #endif
planner._buffer_line( delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder ); planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
#else // cartesian #else // cartesian
planner._buffer_line( ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder ); planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
#endif #endif
} }
@ -525,7 +515,7 @@
static bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) { 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 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 return true; // did not move, so current_position still accurate
const float difference[XYZE] = { // cartesian distances moved in XYZE const float difference[XYZE] = { // cartesian distances moved in XYZE
@ -533,21 +523,21 @@
ltarget[Y_AXIS] - current_position[Y_AXIS], ltarget[Y_AXIS] - current_position[Y_AXIS],
ltarget[Z_AXIS] - current_position[Z_AXIS], ltarget[Z_AXIS] - current_position[Z_AXIS],
ltarget[E_AXIS] - current_position[E_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 const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]); // total horizontal xy distance
#if IS_KINEMATIC #if IS_KINEMATIC
float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate const 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 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 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) NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
#else #else
uint16_t segments = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif #endif
NOLESS( segments, 1 ); // must have at least one segment NOLESS(segments, 1); // must have at least one segment
float inv_segments = 1.0 / segments; // divide once, multiply thereafter const float inv_segments = 1.0 / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s #if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
@ -560,57 +550,53 @@
difference[Y_AXIS] * inv_segments, difference[Y_AXIS] * inv_segments,
difference[Z_AXIS] * inv_segments, difference[Z_AXIS] * inv_segments,
difference[E_AXIS] * inv_segments difference[E_AXIS] * inv_segments
}; };
// Note that E segment distance could vary slightly as z mesh height // Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore. // changes for each segment, but small enough to ignore.
bool above_fade_height = false; const bool above_fade_height = (
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (( planner.z_fade_height != 0 ) && planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
( planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS]) )) { #else
above_fade_height = true; false
} #endif
#endif );
// Only compute leveling per segment if ubl active and target below z_fade_height. // Only compute leveling per segment if ubl active and target below z_fade_height.
if (( ! ubl.state.active ) || ( above_fade_height )) { // no mesh leveling if (!ubl.state.active || above_fade_height) { // no mesh leveling
const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0; const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0;
float seg_dest[XYZE]; // per-segment destination, float seg_dest[XYZE]; // per-segment destination,
COPY_XYZE( seg_dest, current_position ); // starting from current position COPY(seg_dest, current_position); // starting from current position
while (--segments) { while (--segments) {
LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
float ztemp = seg_dest[Z_AXIS]; float ztemp = seg_dest[Z_AXIS];
seg_dest[Z_AXIS] += z_offset; seg_dest[Z_AXIS] += z_offset;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
seg_dest[Z_AXIS] = ztemp; seg_dest[Z_AXIS] = ztemp;
} }
// Since repeated adding segment_distance accumulates small errors, final move to exact destination. // Since repeated adding segment_distance accumulates small errors, final move to exact destination.
COPY_XYZE( seg_dest, ltarget ); COPY(seg_dest, ltarget);
seg_dest[Z_AXIS] += z_offset; seg_dest[Z_AXIS] += z_offset;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
return false; // moved but did not set_current_to_destination(); return false; // moved but did not set_current_to_destination();
} }
// Otherwise perform per-segment leveling // 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 float seg_dest[XYZE]; // per-segment destination, initialize to first segment
LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i]; 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& dx_seg = segment_distance[X_AXIS]; // alias for clarity
const float& dy_seg = segment_distance[Y_AXIS]; 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 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]); ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
do { // for each mesh cell encountered during the move do { // for each mesh cell encountered during the move
@ -621,74 +607,75 @@
// in top of loop and again re-find same adjacent cell and use it, just less efficient // in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area. // for mesh inset area.
int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); 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 ); cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
int8_t cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain( cell_yi, 0, (GRID_MAX_POINTS_Y) - 1 ); 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 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 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 x1 = x0 + MESH_X_DIST; // upper right cell corner
// float y1 = y0 + MESH_Y_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 const 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 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 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 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 cx = rx - x0, // cell-relative x
float cy = ry - y0; // cell-relative y cy = ry - y0; // cell-relative y
float z_x0y0 = ubl.z_values[cell_xi ][cell_yi ]; // z at lower left corner 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 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 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 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_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_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_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 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) const 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) 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_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 const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
float z_cxcy = z_cxy0 + z_cxym * cy; // z height along cx at cy 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
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 // 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 // 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. // each change by a constant for fixed segment lengths.
float z_sxy0 = z_xmy0 * dx_seg; // per-segment adjustment to z_cxy0 const 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 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 do { // for all segments within this mesh cell
z_cxcy += ubl.state.z_offset; z_cxcy += ubl.state.z_offset;
if ( --segments == 0 ) { // this is last segment, use ltarget for exact if (--segments == 0) { // this is last segment, use ltarget for exact
COPY_XYZE( seg_dest, ltarget ); COPY(seg_dest, ltarget);
seg_dest[Z_AXIS] += z_cxcy; seg_dest[Z_AXIS] += z_cxcy;
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
return false; // did not set_current_to_destination() return false; // did not set_current_to_destination()
} }
float z_orig = seg_dest[Z_AXIS]; // remember the pre-leveled segment z value const 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 seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling
ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
seg_dest[Z_AXIS] = z_orig; // restore pre-leveled z before incrementing 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 LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment
cx += dx_seg; cx += dx_seg;
cy += dy_seg; cy += dy_seg;
if ( !WITHIN(cx,0,MESH_X_DIST) || !WITHIN(cy,0,MESH_Y_DIST)) { // done within this cell, break to next 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]); rx = RAW_X_POSITION(seg_dest[X_AXIS]);
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
break; break;