sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Additional UBL fixes, optimizations

Browse Source
This commit is contained in:
Scott Lahteine
2017-03-31 07:57:41 -05:00
parent a5e085cbea
commit 342ee458ae
7 changed files with 177 additions and 216 deletions
Download Patch File Download Diff File Expand all files Collapse all files

181
Marlin/UBL_line_to_destination.cpp
View File

@@ -31,7 +31,14 @@
extern float destination[XYZE];
extern void set_current_to_destination();
extern float destination[];
static void debug_echo_axis(const AxisEnum axis) {
if (current_position[axis] == destination[axis])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
}
void debug_current_and_destination(char *title) {
// if the title message starts with a '!' it is so important, we are going to
@@ -67,32 +74,13 @@
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( ");
if (current_position[X_AXIS] == destination[X_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", ");
if (current_position[Y_AXIS] == destination[Y_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[Y_AXIS], 6);
debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", ");
if (current_position[Z_AXIS] == destination[Z_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[Z_AXIS], 6);
debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", ");
if (current_position[E_AXIS] == destination[E_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[E_AXIS], 6);
debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title);
SERIAL_EOL;
@@ -105,32 +93,37 @@
//}
}
void ubl_line_to_destination(const float &x_end, const float &y_end, const float &z_end, const float &e_end, const float &feed_rate, uint8_t extruder) {
void ubl_line_to_destination(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
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
const float x_start = current_position[X_AXIS],
y_start = current_position[Y_AXIS],
z_start = current_position[Z_AXIS],
e_start = current_position[E_AXIS];
const float start[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
},
end[XYZE] = {
destination[X_AXIS],
destination[Y_AXIS],
destination[Z_AXIS],
destination[E_AXIS]
};
const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_start)),
cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_start)),
cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_end)),
cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_end));
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]));
if (ubl.g26_debug_flag) {
SERIAL_ECHOPGM(" ubl_line_to_destination(xe=");
SERIAL_ECHO(x_end);
SERIAL_ECHOPGM(", ye=");
SERIAL_ECHO(y_end);
SERIAL_ECHOPGM(", ze=");
SERIAL_ECHO(z_end);
SERIAL_ECHOPGM(", ee=");
SERIAL_ECHO(e_end);
SERIAL_ECHOLNPGM(")");
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((char*)"Start of ubl_line_to_destination()");
}
@@ -142,12 +135,12 @@
* But we detect it and isolate it. For now, we just pass along the request.
*/
if (cell_dest_xi < 0 || cell_dest_yi < 0 || cell_dest_xi >= UBL_MESH_NUM_X_POINTS || cell_dest_yi >= UBL_MESH_NUM_Y_POINTS) {
if (!WITHIN(cell_dest_xi, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(cell_dest_yi, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
// 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(x_end, y_end, z_end + ubl.state.z_offset, e_end, 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)
@@ -167,7 +160,7 @@
* 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(x_end) - ubl.mesh_index_to_xpos[cell_dest_xi]) * (1.0 / (MESH_X_DIST)),
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - 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 *
@@ -176,7 +169,7 @@
// 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(y_end) - ubl.mesh_index_to_ypos[cell_dest_yi]) * (1.0 / (MESH_Y_DIST));
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - ubl.mesh_index_to_ypos[cell_dest_yi]) * (1.0 / (MESH_Y_DIST));
float z0 = z1 + (z2 - z1) * yratio;
@@ -186,20 +179,20 @@
*/
/*
z_optimized = z0;
z0 = ubl.get_z_correction(x_end, y_end);
z0 = ubl.get_z_correction(end[X_AXIS], end[Y_AXIS]);
if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"FINAL_MOVE: z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN ");
if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN ");
SERIAL_ECHOPAIR(" x_end=", x_end);
SERIAL_ECHOPAIR(" y_end=", y_end);
SERIAL_ECHOPAIR(" end[X_AXIS]=", end[X_AXIS]);
SERIAL_ECHOPAIR(" end[Y_AXIS]=", end[Y_AXIS]);
SERIAL_ECHOPAIR(" z0=", z0);
SERIAL_ECHOPAIR(" z_optimized=", z_optimized);
SERIAL_ECHOPAIR(" err=",fabs(z_optimized - z0));
SERIAL_EOL;
}
//*/
z0 *= ubl.fade_scaling_factor_for_z(z_end);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -210,7 +203,7 @@
*/
if (isnan(z0)) z0 = 0.0;
planner.buffer_line(x_end, y_end, z_end + z0 + ubl.state.z_offset, e_end, 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((char*)"FINAL_MOVE in ubl_line_to_destination()");
@@ -227,8 +220,8 @@
* blocks of code:
*/
const float dx = x_end - x_start,
dy = y_end - y_start;
const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1 : 0;
@@ -251,8 +244,8 @@
const bool use_x_dist = adx > ady;
float on_axis_distance = use_x_dist ? dx : dy,
e_position = e_end - e_start,
z_position = z_end - z_start;
e_position = end[E_AXIS] - start[E_AXIS],
z_position = end[Z_AXIS] - start[Z_AXIS];
const float e_normalized_dist = e_position / on_axis_distance,
z_normalized_dist = z_position / on_axis_distance;
@@ -260,7 +253,7 @@
int current_xi = cell_start_xi, current_yi = cell_start_yi;
const float m = dy / dx,
c = y_start - m * x_start;
c = start[Y_AXIS] - m * start[X_AXIS];
const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance),
inf_m_flag = NEAR_ZERO(dx);
@@ -281,9 +274,9 @@
* else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line
*/
const float x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m;
const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi);
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
/**
* Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -305,7 +298,7 @@
}
//*/
z0 *= ubl.fade_scaling_factor_for_z(z_end);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -324,15 +317,15 @@
* 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.
*/
if (y != y_start) {
if (y != start[Y_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = y - y_start; // we don't need to check if the extruder position
e_position = e_start + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a vertical move
z_position = z_start + on_axis_distance * z_normalized_dist;
on_axis_distance = y - start[Y_AXIS]; // we don't need to check if the extruder position
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a vertical move
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = e_start;
z_position = z_start;
e_position = start[E_AXIS];
z_position = start[Z_AXIS];
}
planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -345,7 +338,7 @@
//
// 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.
//
if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end)
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_to_destination();
@@ -368,7 +361,7 @@
const float next_mesh_line_x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi]),
y = m * next_mesh_line_x + c; // Calculate X at the next Y mesh line
float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi);
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
/**
* Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -390,7 +383,7 @@
}
//*/
z0 = z0 * ubl.fade_scaling_factor_for_z(z_end);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -409,15 +402,15 @@
* 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.
*/
if (x != x_start) {
if (x != start[X_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = x - x_start; // we don't need to check if the extruder position
e_position = e_start + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = z_start + on_axis_distance * z_normalized_dist;
on_axis_distance = x - start[X_AXIS]; // we don't need to check if the extruder position
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = e_start;
z_position = z_start;
e_position = start[E_AXIS];
z_position = start[Z_AXIS];
}
planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -427,7 +420,7 @@
if (ubl.g26_debug_flag)
debug_current_and_destination((char*)"horizontal move done in ubl_line_to_destination()");
if (current_position[X_AXIS] != x_end || current_position[Y_AXIS] != y_end)
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_to_destination();
@@ -454,16 +447,16 @@
const float next_mesh_line_x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi + dxi]),
next_mesh_line_y = LOGICAL_Y_POSITION(ubl.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 (we don't have to worry
// about m being equal to 0.0 If this was the case, we would have
// detected this as a vertical line move up above and we wouldn't
// be down here doing a generic type of move.
x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero.
// If that was the case, it was already detected
// as a vertical line move above.)
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.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi);
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
/**
* Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -486,7 +479,7 @@
}
//*/
z0 *= ubl.fade_scaling_factor_for_z(z_end);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -498,13 +491,13 @@
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? x - x_start : next_mesh_line_y - y_start;
e_position = e_start + on_axis_distance * e_normalized_dist;
z_position = z_start + on_axis_distance * z_normalized_dist;
on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = e_start;
z_position = z_start;
e_position = start[E_AXIS];
z_position = start[Z_AXIS];
}
planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
current_yi += dyi;
@@ -514,7 +507,7 @@
//
// Yes! Crossing a X Mesh Line next
//
float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag);
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
/**
* Debug code to use non-optimized get_z_correction() and to do a sanity check
@@ -536,7 +529,7 @@
}
//*/
z0 *= ubl.fade_scaling_factor_for_z(z_end);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -548,13 +541,13 @@
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? next_mesh_line_x - x_start : y - y_start;
e_position = e_start + on_axis_distance * e_normalized_dist;
z_position = z_start + on_axis_distance * z_normalized_dist;
on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = e_start;
z_position = z_start;
e_position = start[E_AXIS];
z_position = start[Z_AXIS];
}
planner.buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
@@ -566,7 +559,7 @@
if (ubl.g26_debug_flag)
debug_current_and_destination((char*)"generic move done in ubl_line_to_destination()");
if (current_position[0] != x_end || current_position[1] != y_end)
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_to_destination();