bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Embed G26/G29 in ubl class, with enhancements

Browse Source
This commit is contained in:
Scott Lahteine
2017-05-22 14:41:09 -05:00
parent c99bd69889
commit 85b967657e
7 changed files with 513 additions and 482 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
Marlin/ubl_motion.cpp
View File

@ -85,7 +85,7 @@
}
void ubl_line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
void unified_bed_leveling::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
@ -104,19 +104,19 @@
destination[E_AXIS]
};
const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(start[X_AXIS])),
cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])),
cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(end[X_AXIS])),
cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS]));
const int cell_start_xi = get_cell_index_x(RAW_X_POSITION(start[X_AXIS])),
cell_start_yi = get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])),
cell_dest_xi = get_cell_index_x(RAW_X_POSITION(end[X_AXIS])),
cell_dest_yi = get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS]));
if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl_line_to_destination(xe=", end[X_AXIS]);
if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
SERIAL_CHAR(')');
SERIAL_EOL;
debug_current_and_destination(PSTR("Start of ubl_line_to_destination()"));
debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
}
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
@ -132,11 +132,11 @@
// 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] + state.z_offset, end[E_AXIS], feed_rate, extruder);
set_current_to_destination();
if (ubl.g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl_line_to_destination()"));
if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
return;
}
@ -152,20 +152,20 @@
* to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
*/
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - pgm_read_float(&ubl.mesh_index_to_xpos[cell_dest_xi])) * (1.0 / (MESH_X_DIST)),
z1 = ubl.z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(ubl.z_values[cell_dest_xi + 1][cell_dest_yi ] - ubl.z_values[cell_dest_xi][cell_dest_yi ]),
z2 = ubl.z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(ubl.z_values[cell_dest_xi + 1][cell_dest_yi + 1] - ubl.z_values[cell_dest_xi][cell_dest_yi + 1]);
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)),
z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
// are going to apply the Y-Distance into the cell to interpolate the final Z correction.
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - pgm_read_float(&ubl.mesh_index_to_ypos[cell_dest_yi])) * (1.0 / (MESH_Y_DIST));
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
float z0 = z1 + (z2 - z1) * yratio;
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -176,10 +176,10 @@
*/
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 + 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()"));
if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
set_current_to_destination();
return;
@ -240,7 +240,7 @@
current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi;
const float next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi]));
const float next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi));
/**
* if the slope of the line is infinite, we won't do the calculations
@ -249,9 +249,9 @@
*/
const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -262,7 +262,7 @@
*/
if (isnan(z0)) z0 = 0.0;
const float y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi]));
const float y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi));
/**
* Without this check, it is possible for the algorithm to generate a zero length move in the case
@ -281,12 +281,12 @@
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 + state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
if (ubl.g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl_line_to_destination()"));
if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
//
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
@ -311,12 +311,12 @@
// edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi;
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi])),
const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi)),
y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -327,7 +327,7 @@
*/
if (isnan(z0)) z0 = 0.0;
const float x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi]));
const float x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi));
/**
* Without this check, it is possible for the algorithm to generate a zero length move in the case
@ -346,12 +346,12 @@
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 + state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
if (ubl.g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl_line_to_destination()"));
if (g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -377,8 +377,8 @@
while (xi_cnt > 0 || yi_cnt > 0) {
const float next_mesh_line_x = LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi + dxi])),
next_mesh_line_y = LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi + dyi])),
const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi + dxi)),
next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi + dyi)),
y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero.
@ -387,9 +387,9 @@
if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
// Yes! Crossing a Y Mesh Line next
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -409,15 +409,15 @@
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 + state.z_offset, e_position, feed_rate, extruder);
current_yi += dyi;
yi_cnt--;
}
else {
// Yes! Crossing a X Mesh Line next
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -438,7 +438,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 + state.z_offset, e_position, feed_rate, extruder);
current_xi += dxi;
xi_cnt--;
}
@ -446,8 +446,8 @@
if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
}
if (ubl.g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl_line_to_destination()"));
if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -502,7 +502,7 @@
* 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) {
static bool unified_bed_leveling::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
@ -554,9 +554,9 @@
// 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 (!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 = state.active ? state.z_offset : 0.0;
float seg_dest[XYZE]; // per-segment destination,
COPY_XYZE(seg_dest, current_position); // starting from current position
@ -579,7 +579,7 @@
// Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = ubl.fade_scaling_factor_for_z(ltarget[Z_AXIS]);
const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
#endif
float seg_dest[XYZE]; // per-segment destination, initialize to first segment
@ -591,7 +591,7 @@
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
for(;;) { // 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)
@ -606,19 +606,19 @@
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
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
const float x0 = pgm_read_float(&(mesh_index_to_xpos[cell_xi ])), // 64 byte table lookup avoids mul+add
y0 = pgm_read_float(&(mesh_index_to_ypos[cell_yi ])), // 64 byte table lookup avoids mul+add
x1 = pgm_read_float(&(mesh_index_to_xpos[cell_xi+1])), // 64 byte table lookup avoids mul+add
y1 = pgm_read_float(&(mesh_index_to_ypos[cell_yi+1])); // 64 byte table lookup avoids mul+add
float cx = rx - x0, // cell-relative x
cy = ry - y0, // cell-relative y
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
z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
z_x1y1 = 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 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
@ -642,7 +642,7 @@
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
for(;;) { // for all segments within this mesh cell
float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy
@ -650,7 +650,7 @@
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
#endif
z_cxcy += ubl.state.z_offset; // add fixed mesh offset from G29 Z
z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z
if (--segments == 0) { // if this is last segment, use ltarget for exact
COPY_XYZE(seg_dest, ltarget);
@ -681,9 +681,9 @@
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
} 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
} // segment loop
} // cell loop
}
#endif // UBL_DELTA