|
|
|
|
@ -474,20 +474,10 @@
|
|
|
|
|
set_current_to_destination();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#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 UBL_DELTA
|
|
|
|
|
|
|
|
|
|
#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;
|
|
|
|
|
static float scara_feed_factor, scara_oldA, scara_oldB;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
|
|
|
|
#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);
|
|
|
|
|
float adiff = abs(delta[A_AXIS] - scara_oldA),
|
|
|
|
|
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 );
|
|
|
|
|
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 );
|
|
|
|
|
planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
@ -525,7 +515,7 @@
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
const float difference[XYZE] = { // cartesian distances moved in XYZE
|
|
|
|
|
@ -533,21 +523,21 @@
|
|
|
|
|
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
|
|
|
|
|
const float cartesian_xy_mm = HYPOT(difference[X_AXIS], 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)
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
NOLESS(segments, 1); // must have at least one segment
|
|
|
|
|
const 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;
|
|
|
|
|
@ -560,57 +550,53 @@
|
|
|
|
|
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
|
|
|
|
|
const bool above_fade_height = (
|
|
|
|
|
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
|
|
|
|
planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
|
|
|
|
|
#else
|
|
|
|
|
false
|
|
|
|
|
#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
|
|
|
|
|
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
|
|
|
|
|
float seg_dest[XYZE]; // per-segment destination,
|
|
|
|
|
COPY(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 );
|
|
|
|
|
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 );
|
|
|
|
|
COPY(seg_dest, ltarget);
|
|
|
|
|
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();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 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]);
|
|
|
|
|
float rx = RAW_X_POSITION(seg_dest[X_AXIS]), // assume raw vs logical coordinates shifted but not scaled.
|
|
|
|
|
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
// 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_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
|
|
|
|
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_yi = constrain( cell_yi, 0, (GRID_MAX_POINTS_Y) - 1 );
|
|
|
|
|
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 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 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
|
|
|
|
|
const float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi ])), // 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
|
|
|
|
|
x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+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
|
|
|
|
|
float cy = ry - y0; // cell-relative y
|
|
|
|
|
cx = rx - x0, // cell-relative x
|
|
|
|
|
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
|
|
|
|
|
float z_x0y0 = ubl.z_values[cell_xi ][cell_yi ], // z at lower left corner
|
|
|
|
|
z_x1y0 = ubl.z_values[cell_xi+1][cell_yi ], // z at upper left corner
|
|
|
|
|
z_x0y1 = ubl.z_values[cell_xi ][cell_yi+1], // z at lower 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_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
|
|
|
|
|
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)
|
|
|
|
|
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower 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_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_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
|
|
|
|
|
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
|
|
|
|
|
// 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
|
|
|
|
|
const float z_sxy0 = z_xmy0 * dx_seg, // per-segment adjustment to z_cxy0
|
|
|
|
|
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 );
|
|
|
|
|
if (--segments == 0) { // this is last segment, use ltarget for exact
|
|
|
|
|
COPY(seg_dest, ltarget);
|
|
|
|
|
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()
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
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;
|
|
|
|
|
|
Scott Lahteine
8 years ago