bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 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
Download Patch File Download Diff File Expand all files Collapse all files

149
Marlin/ubl_motion.cpp
View File

@ -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;