|
|
|
@ -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
|
|
|
|
|
|
|
|
|
oldmcg
8 years ago
Roxy-3D