3DPrinting/MarlinFirmware
1
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2024-11-27 13:56:24 +00:00
Code Issues 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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.travis.yml
View File

@ -120,7 +120,7 @@ script:
# Test a simple build of AUTO_BED_LEVELING_UBL # Test a simple build of AUTO_BED_LEVELING_UBL
# #
- restore_configs - restore_configs
- opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL - opt_enable AUTO_BED_LEVELING_UBL UBL_G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT FIX_MOUNTED_PROBE EEPROM_SETTINGS G3D_PANEL
- build_marlin - build_marlin
# #
# Test a Sled Z Probe # Test a Sled Z Probe

93
Marlin/G26_Mesh_Validation_Tool.cpp
View File

@ -47,7 +47,7 @@
#define OOZE_AMOUNT 0.3 #define OOZE_AMOUNT 0.3
#define SIZE_OF_INTERSECTION_CIRCLES 5 #define SIZE_OF_INTERSECTION_CIRCLES 5
#define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle. This number should be #define SIZE_OF_CROSSHAIRS 3 // crosshairs inside the circle. This number should be
// less than SIZE_OR_INTERSECTION_CIRCLES // less than SIZE_OR_INTERSECTION_CIRCLES
/** /**
@ -132,12 +132,12 @@
void line_to_destination(float ); void line_to_destination(float );
void gcode_G28(); void gcode_G28();
void sync_plan_position_e(); void sync_plan_position_e();
void un_retract_filament(); void un_retract_filament(float where[XYZE]);
void retract_filament(); void retract_filament(float where[XYZE]);
void look_for_lines_to_connect(); void look_for_lines_to_connect();
bool parse_G26_parameters(); bool parse_G26_parameters();
void move_to(const float&, const float&, const float&, const float&) ; void move_to(const float&, const float&, const float&, const float&) ;
void print_line_from_here_to_there(float sx, float sy, float sz, float ex, float ey, float ez); void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
bool turn_on_heaters(); bool turn_on_heaters();
bool prime_nozzle(); bool prime_nozzle();
void chirp_at_user(); void chirp_at_user();
@ -154,8 +154,6 @@
float valid_trig_angle(float); float valid_trig_angle(float);
mesh_index_pair find_closest_circle_to_print(float, float); mesh_index_pair find_closest_circle_to_print(float, float);
void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t);
//uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF); /* needed for the old mesh_buffer_line() routine */
static float extrusion_multiplier = EXTRUSION_MULTIPLIER, static float extrusion_multiplier = EXTRUSION_MULTIPLIER,
retraction_multiplier = RETRACTION_MULTIPLIER, retraction_multiplier = RETRACTION_MULTIPLIER,
@ -359,7 +357,7 @@
lcd_reset_alert_level(); lcd_reset_alert_level();
lcd_setstatuspgm(PSTR("Leaving G26")); lcd_setstatuspgm(PSTR("Leaving G26"));
retract_filament(); retract_filament(destination);
destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES;
//debug_current_and_destination((char*)"ready to do Z-Raise."); //debug_current_and_destination((char*)"ready to do Z-Raise.");
@ -445,18 +443,12 @@
// We found two circles that need a horizontal line to connect them // We found two circles that need a horizontal line to connect them
// Print it! // Print it!
// //
sx = ubl.mesh_index_to_xpos[i]; sx = ubl.mesh_index_to_xpos[ i ] + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge
sx = sx + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the right edge of the circle ex = ubl.mesh_index_to_xpos[i + 1] - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge
sy = ubl.mesh_index_to_ypos[j];
ex = ubl.mesh_index_to_xpos[i + 1]; sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1);
ex = ex - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the left edge of the circle sy = ey = constrain(ubl.mesh_index_to_ypos[j], Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = sy;
sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
@ -468,7 +460,7 @@
//debug_current_and_destination((char*)"Connecting horizontal line."); //debug_current_and_destination((char*)"Connecting horizontal line.");
} }
print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height); print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again
} }
} }
@ -482,17 +474,11 @@
// We found two circles that need a vertical line to connect them // We found two circles that need a vertical line to connect them
// Print it! // Print it!
// //
sx = ubl.mesh_index_to_xpos[i]; sy = ubl.mesh_index_to_ypos[ j ] + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge
sy = ubl.mesh_index_to_ypos[j]; ey = ubl.mesh_index_to_ypos[j + 1] - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge
sy = sy + SIZE_OF_INTERSECTION_CIRCLES - SIZE_OF_CROSS_HAIRS; // get the top edge of the circle
ex = sx; sx = ex = constrain(ubl.mesh_index_to_xpos[i], X_MIN_POS + 1, X_MAX_POS - 1);
ey = ubl.mesh_index_to_ypos[j + 1];
ey = ey - SIZE_OF_INTERSECTION_CIRCLES + SIZE_OF_CROSS_HAIRS; // get the bottom edge of the circle
sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
@ -504,7 +490,7 @@
SERIAL_EOL; SERIAL_EOL;
debug_current_and_destination((char*)"Connecting vertical line."); debug_current_and_destination((char*)"Connecting vertical line.");
} }
print_line_from_here_to_there(sx, sy, layer_height, ex, ey, layer_height); print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height);
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again
} }
} }
@ -533,7 +519,7 @@
destination[Z_AXIS] = z; // We know the last_z==z or we wouldn't be in this block of code. destination[Z_AXIS] = z; // We know the last_z==z or we wouldn't be in this block of code.
destination[E_AXIS] = current_position[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS];
ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0); ubl_line_to_destination(feed_value, 0);
stepper.synchronize(); stepper.synchronize();
set_destination_to_current(); set_destination_to_current();
@ -553,7 +539,7 @@
//if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() doing last move"); //if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() doing last move");
ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feed_value, 0); ubl_line_to_destination(feed_value, 0);
//if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() after last move"); //if (ubl.g26_debug_flag) debug_current_and_destination((char*)" in move_to() after last move");
@ -562,18 +548,18 @@
} }
void retract_filament() { void retract_filament(float where[XYZE]) {
if (!g26_retracted) { // Only retract if we are not already retracted! if (!g26_retracted) { // Only retract if we are not already retracted!
g26_retracted = true; g26_retracted = true;
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract."); //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Decided to do retract.");
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], -1.0 * retraction_multiplier); move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], -1.0 * retraction_multiplier);
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Retraction done."); //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Retraction done.");
} }
} }
void un_retract_filament() { void un_retract_filament(float where[XYZE]) {
if (g26_retracted) { // Only un-retract if we are retracted. if (g26_retracted) { // Only un-retract if we are retracted.
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 1.2 * retraction_multiplier); move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], 1.2 * retraction_multiplier);
g26_retracted = false; g26_retracted = false;
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" unretract done."); //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" unretract done.");
} }
@ -594,7 +580,7 @@
* segment of a 'circle'. The time this requires is very short and is easily saved by the other * segment of a 'circle'. The time this requires is very short and is easily saved by the other
* cases where the optimization comes into play. * cases where the optimization comes into play.
*/ */
void print_line_from_here_to_there( float sx, float sy, float sz, float ex, float ey, float ez) { void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment
dy_s = current_position[Y_AXIS] - sy, dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2 dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2
@ -603,31 +589,26 @@
dy_e = current_position[Y_AXIS] - ey, dy_e = current_position[Y_AXIS] - ey,
dist_end = HYPOT2(dx_e, dy_e), dist_end = HYPOT2(dx_e, dy_e),
dx = ex - sx, line_length = HYPOT(ex - sx, ey - sy);
dy = ey - sy,
line_length = HYPOT(dx, dy);
// If the end point of the line is closer to the nozzle, we are going to // If the end point of the line is closer to the nozzle, flip the direction,
// flip the direction of this line. We will print it from the end to the start. // moving from the end to the start. On very small lines the optimization isn't worth it.
// On very small lines we don't do the optimization because it just isn't worth it.
//
if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(line_length)) { if (dist_end < dist_start && (SIZE_OF_INTERSECTION_CIRCLES) < abs(line_length)) {
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Reversing start and end of print_line_from_here_to_there()"); //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" Reversing start and end of print_line_from_here_to_there()");
print_line_from_here_to_there(ex, ey, ez, sx, sy, sz); return print_line_from_here_to_there(ex, ey, ez, sx, sy, sz);
return;
} }
// Now decide if we should retract. // Decide whether to retract.
if (dist_start > 2.0) { if (dist_start > 2.0) {
retract_filament(); retract_filament(destination);
//if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" filament retracted."); //if (ubl.g26_debug_flag) SERIAL_ECHOLNPGM(" filament retracted.");
} }
move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion
const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier; const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier;
un_retract_filament(); un_retract_filament(destination);
//if (ubl.g26_debug_flag) { //if (ubl.g26_debug_flag) {
// SERIAL_ECHOLNPGM(" doing printing move."); // SERIAL_ECHOLNPGM(" doing printing move.");
@ -814,6 +795,7 @@
lcd_setstatuspgm(PSTR("")); lcd_setstatuspgm(PSTR(""));
lcd_quick_feedback(); lcd_quick_feedback();
#endif #endif
return UBL_OK; return UBL_OK;
} }
@ -832,9 +814,8 @@
set_destination_to_current(); set_destination_to_current();
un_retract_filament(); // Lets make sure the G26 command doesn't think the filament is un_retract_filament(destination); // Make sure G26 doesn't think the filament is retracted().
// retracted(). We are here because we want to prime the nozzle.
// So let's just unretract just to be sure.
while (!ubl_lcd_clicked()) { while (!ubl_lcd_clicked()) {
chirp_at_user(); chirp_at_user();
destination[E_AXIS] += 0.25; destination[E_AXIS] += 0.25;
@ -842,10 +823,7 @@
Total_Prime += 0.25; Total_Prime += 0.25;
if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR; if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR;
#endif #endif
ubl_line_to_destination( ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
);
stepper.synchronize(); // Without this synchronize, the purge is more consistent, stepper.synchronize(); // Without this synchronize, the purge is more consistent,
// but because the planner has a buffer, we won't be able // but because the planner has a buffer, we won't be able
@ -874,13 +852,10 @@
#endif #endif
set_destination_to_current(); set_destination_to_current();
destination[E_AXIS] += prime_length; destination[E_AXIS] += prime_length;
ubl_line_to_destination( ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0);
destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0
);
stepper.synchronize(); stepper.synchronize();
set_destination_to_current(); set_destination_to_current();
retract_filament(); retract_filament(destination);
} }
return UBL_OK; return UBL_OK;

6
Marlin/Marlin_main.cpp
View File

@ -9901,11 +9901,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#elif ENABLED(AUTO_BED_LEVELING_UBL) #elif ENABLED(AUTO_BED_LEVELING_UBL)
if (ubl.state.active) { if (ubl.state.active) {
// ubl_line_to_destination(MMS_SCALED(feedrate_mm_s)); ubl_line_to_destination(MMS_SCALED(feedrate_mm_s), active_extruder);
ubl_line_to_destination(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS],
// (feedrate*(1.0/60.0))*(feedrate_percentage*(1.0/100.0) ), active_extruder);
MMS_SCALED(feedrate_mm_s), active_extruder);
return false; return false;
} }

72
Marlin/UBL.h
View File

@ -43,7 +43,7 @@
bool ubl_lcd_clicked(); bool ubl_lcd_clicked();
void probe_entire_mesh(const float&, const float&, const bool, const bool, const bool); void probe_entire_mesh(const float&, const float&, const bool, const bool, const bool);
void debug_current_and_destination(char *title); void debug_current_and_destination(char *title);
void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t); void ubl_line_to_destination(const float&, uint8_t);
void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool); void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&); vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&);
float measure_business_card_thickness(const float&); float measure_business_card_thickness(const float&);
@ -193,22 +193,16 @@
* multiplications. * multiplications.
*/ */
static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
const float delta_z = (z2 - z1), return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
delta_a = (a0 - a1) / (a2 - a1);
return z1 + delta_a * delta_z;
} }
/** /**
* get_z_correction_at_Y_intercept(float x0, int x1_i, int yi) only takes * z_correction_for_x_on_horizontal_mesh_line is an optimization for
* three parameters. It assumes the x0 point is on a Mesh line denoted by yi. In theory * the rare occasion when a point lies exactly on a Mesh line (denoted by index yi).
* we could use get_cell_index_x(float x) to obtain the 2nd parameter x1_i but any code calling
* the get_z_correction_along_vertical_mesh_line_at_specific_X routine will already have
* the X index of the x0 intersection available and we don't want to perform any extra floating
* point operations.
*/ */
static inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(const float &x0, const int x1_i, const int yi) { static inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
if (x1_i < 0 || yi < 0 || x1_i >= UBL_MESH_NUM_X_POINTS || yi >= UBL_MESH_NUM_Y_POINTS) { if (!WITHIN(x1_i, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(yi, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
SERIAL_ECHOPAIR("? in get_z_correction_along_horizontal_mesh_line_at_specific_X(x0=", x0); SERIAL_ECHOPAIR("? in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
SERIAL_ECHOPAIR(",x1_i=", x1_i); SERIAL_ECHOPAIR(",x1_i=", x1_i);
SERIAL_ECHOPAIR(",yi=", yi); SERIAL_ECHOPAIR(",yi=", yi);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
@ -216,20 +210,18 @@
return NAN; return NAN;
} }
const float xratio = (RAW_X_POSITION(x0) - mesh_index_to_xpos[x1_i]) * (1.0 / (MESH_X_DIST)), const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos[x1_i]) * (1.0 / (MESH_X_DIST)),
z1 = z_values[x1_i][yi], z1 = z_values[x1_i][yi];
z2 = z_values[x1_i + 1][yi],
dz = (z2 - z1);
return z1 + xratio * dz; return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
} }
// //
// See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X // See comments above for z_correction_for_x_on_horizontal_mesh_line
// //
static inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(const float &y0, const int xi, const int y1_i) { static inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
if (xi < 0 || y1_i < 0 || xi >= UBL_MESH_NUM_X_POINTS || y1_i >= UBL_MESH_NUM_Y_POINTS) { if (!WITHIN(xi, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(y1_i, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_X(y0=", y0); SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_x(ly0=", ly0);
SERIAL_ECHOPAIR(", x1_i=", xi); SERIAL_ECHOPAIR(", x1_i=", xi);
SERIAL_ECHOPAIR(", yi=", y1_i); SERIAL_ECHOPAIR(", yi=", y1_i);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
@ -237,12 +229,10 @@
return NAN; return NAN;
} }
const float yratio = (RAW_Y_POSITION(y0) - mesh_index_to_ypos[y1_i]) * (1.0 / (MESH_Y_DIST)), const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos[y1_i]) * (1.0 / (MESH_Y_DIST)),
z1 = z_values[xi][y1_i], z1 = z_values[xi][y1_i];
z2 = z_values[xi][y1_i + 1],
dz = (z2 - z1);
return z1 + yratio * dz; return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
} }
/** /**
@ -251,14 +241,14 @@
* Z-Height at both ends. Then it does a linear interpolation of these heights based * Z-Height at both ends. Then it does a linear interpolation of these heights based
* on the Y position within the cell. * on the Y position within the cell.
*/ */
static float get_z_correction(const float &x0, const float &y0) { static float get_z_correction(const float &lx0, const float &ly0) {
const int8_t cx = get_cell_index_x(RAW_X_POSITION(x0)), const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
cy = get_cell_index_y(RAW_Y_POSITION(y0)); cy = get_cell_index_y(RAW_Y_POSITION(ly0));
if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) { if (!WITHIN(cx, 0, UBL_MESH_NUM_X_POINTS - 1) || !WITHIN(cy, 0, UBL_MESH_NUM_Y_POINTS - 1)) {
SERIAL_ECHOPAIR("? in get_z_correction(x0=", x0); SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
SERIAL_ECHOPAIR(", y0=", y0); SERIAL_ECHOPAIR(", ly0=", ly0);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL; SERIAL_EOL;
@ -269,21 +259,21 @@
return 0.0; // this used to return state.z_offset return 0.0; // this used to return state.z_offset
} }
const float z1 = calc_z0(RAW_X_POSITION(x0), const float z1 = calc_z0(RAW_X_POSITION(lx0),
mesh_index_to_xpos[cx], z_values[cx][cy], mesh_index_to_xpos[cx], z_values[cx][cy],
mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy]), mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy]),
z2 = calc_z0(RAW_X_POSITION(x0), z2 = calc_z0(RAW_X_POSITION(lx0),
mesh_index_to_xpos[cx], z_values[cx][cy + 1], mesh_index_to_xpos[cx], z_values[cx][cy + 1],
mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]); mesh_index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]);
float z0 = calc_z0(RAW_Y_POSITION(y0), float z0 = calc_z0(RAW_Y_POSITION(ly0),
mesh_index_to_ypos[cy], z1, mesh_index_to_ypos[cy], z1,
mesh_index_to_ypos[cy + 1], z2); mesh_index_to_ypos[cy + 1], z2);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
SERIAL_ECHOPAIR(" raw get_z_correction(", x0); SERIAL_ECHOPAIR(" raw get_z_correction(", lx0);
SERIAL_CHAR(',') SERIAL_CHAR(',')
SERIAL_ECHO(y0); SERIAL_ECHO(ly0);
SERIAL_ECHOPGM(") = "); SERIAL_ECHOPGM(") = ");
SERIAL_ECHO_F(z0, 6); SERIAL_ECHO_F(z0, 6);
} }
@ -305,9 +295,9 @@
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", x0); SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", lx0);
SERIAL_CHAR(','); SERIAL_CHAR(',');
SERIAL_ECHO(y0); SERIAL_ECHO(ly0);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL; SERIAL_EOL;
} }
@ -327,7 +317,7 @@
*/ */
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) { static FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
const float rz = RAW_Z_POSITION(lz); const float rz = RAW_Z_POSITION(lz);
if (last_specified_z != rz) { if (last_specified_z != rz) {
last_specified_z = rz; last_specified_z = rz;

31
Marlin/UBL_G29.cpp
View File

@ -515,16 +515,23 @@
} }
if (code_seen('T')) { if (code_seen('T')) {
float z1 = probe_pt(ubl_3_point_1_X, ubl_3_point_1_Y, false /*Stow Flag*/, g29_verbose_level), const float lx1 = LOGICAL_X_POSITION(ubl_3_point_1_X),
z2 = probe_pt(ubl_3_point_2_X, ubl_3_point_2_Y, false /*Stow Flag*/, g29_verbose_level), lx2 = LOGICAL_X_POSITION(ubl_3_point_2_X),
z3 = probe_pt(ubl_3_point_3_X, ubl_3_point_3_Y, true /*Stow Flag*/, g29_verbose_level); lx3 = LOGICAL_X_POSITION(ubl_3_point_3_X),
ly1 = LOGICAL_Y_POSITION(ubl_3_point_1_Y),
ly2 = LOGICAL_Y_POSITION(ubl_3_point_2_Y),
ly3 = LOGICAL_Y_POSITION(ubl_3_point_3_Y);
float z1 = probe_pt(lx1, ly1, false /*Stow Flag*/, g29_verbose_level),
z2 = probe_pt(lx2, ly2, false /*Stow Flag*/, g29_verbose_level),
z3 = probe_pt(lx3, ly3, true /*Stow Flag*/, g29_verbose_level);
// We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean // We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
// the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is) // the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is)
z1 -= ubl.get_z_correction(ubl_3_point_1_X, ubl_3_point_1_Y); z1 -= ubl.get_z_correction(lx1, ly1);
z2 -= ubl.get_z_correction(ubl_3_point_2_X, ubl_3_point_2_Y); z2 -= ubl.get_z_correction(lx2, ly2);
z3 -= ubl.get_z_correction(ubl_3_point_3_X, ubl_3_point_3_Y); z3 -= ubl.get_z_correction(lx3, ly3);
do_blocking_move_to_xy((X_MAX_POS - (X_MIN_POS)) / 2.0, (Y_MAX_POS - (Y_MIN_POS)) / 2.0); do_blocking_move_to_xy((X_MAX_POS - (X_MIN_POS)) / 2.0, (Y_MAX_POS - (Y_MIN_POS)) / 2.0);
tilt_mesh_based_on_3pts(z1, z2, z3); tilt_mesh_based_on_3pts(z1, z2, z3);
@ -778,17 +785,17 @@
); );
} }
vector_3 tilt_mesh_based_on_3pts(const float &pt1, const float &pt2, const float &pt3) { vector_3 tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
float c, d, t; float c, d, t;
int i, j; int i, j;
vector_3 v1 = vector_3( (ubl_3_point_1_X - ubl_3_point_2_X), vector_3 v1 = vector_3( (ubl_3_point_1_X - ubl_3_point_2_X),
(ubl_3_point_1_Y - ubl_3_point_2_Y), (ubl_3_point_1_Y - ubl_3_point_2_Y),
(pt1 - pt2) ), (z1 - z2) ),
v2 = vector_3( (ubl_3_point_3_X - ubl_3_point_2_X), v2 = vector_3( (ubl_3_point_3_X - ubl_3_point_2_X),
(ubl_3_point_3_Y - ubl_3_point_2_Y), (ubl_3_point_3_Y - ubl_3_point_2_Y),
(pt3 - pt2) ), (z3 - z2) ),
normal = vector_3::cross(v1, v2); normal = vector_3::cross(v1, v2);
@ -810,7 +817,7 @@
// All of 3 of these points should give us the same d constant // All of 3 of these points should give us the same d constant
// //
t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y; t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y;
d = t + normal.z * pt1; d = t + normal.z * z1;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 1st point: "); SERIAL_ECHOPGM("d from 1st point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
@ -818,7 +825,7 @@
SERIAL_ECHO_F(c, 6); SERIAL_ECHO_F(c, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y; t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y;
d = t + normal.z * pt2; d = t + normal.z * z2;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 2nd point: "); SERIAL_ECHOPGM("d from 2nd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
@ -826,7 +833,7 @@
SERIAL_ECHO_F(c, 6); SERIAL_ECHO_F(c, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y; t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y;
d = t + normal.z * pt3; d = t + normal.z * z3;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 3rd point: "); SERIAL_ECHOPGM("d from 3rd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);

181
Marlin/UBL_line_to_destination.cpp
View File

@ -31,7 +31,14 @@
extern float destination[XYZE]; extern float destination[XYZE];
extern void set_current_to_destination(); 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) { void debug_current_and_destination(char *title) {
// if the title message starts with a '!' it is so important, we are going to // if the title message starts with a '!' it is so important, we are going to
@ -67,32 +74,13 @@
SERIAL_ECHOPGM(", "); SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6); SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( "); SERIAL_ECHOPGM(" ) destination=( ");
if (current_position[X_AXIS] == destination[X_AXIS]) debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
SERIAL_ECHOPGM(", "); SERIAL_ECHOPGM(", ");
debug_echo_axis(Y_AXIS);
if (current_position[Y_AXIS] == destination[Y_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[Y_AXIS], 6);
SERIAL_ECHOPGM(", "); SERIAL_ECHOPGM(", ");
debug_echo_axis(Z_AXIS);
if (current_position[Z_AXIS] == destination[Z_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[Z_AXIS], 6);
SERIAL_ECHOPGM(", "); SERIAL_ECHOPGM(", ");
debug_echo_axis(E_AXIS);
if (current_position[E_AXIS] == destination[E_AXIS])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[E_AXIS], 6);
SERIAL_ECHOPGM(" ) "); SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title); SERIAL_ECHO(title);
SERIAL_EOL; 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 * 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 * 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 * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/ */
const float x_start = current_position[X_AXIS], const float start[XYZE] = {
y_start = current_position[Y_AXIS], current_position[X_AXIS],
z_start = current_position[Z_AXIS], current_position[Y_AXIS],
e_start = current_position[E_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)), 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(y_start)), 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(x_end)), 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(y_end)); cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS]));
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
SERIAL_ECHOPGM(" ubl_line_to_destination(xe="); SERIAL_ECHOPAIR(" ubl_line_to_destination(xe=", end[X_AXIS]);
SERIAL_ECHO(x_end); SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
SERIAL_ECHOPGM(", ye="); SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
SERIAL_ECHO(y_end); SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
SERIAL_ECHOPGM(", ze="); SERIAL_CHAR(')');
SERIAL_ECHO(z_end); SERIAL_EOL;
SERIAL_ECHOPGM(", ee=");
SERIAL_ECHO(e_end);
SERIAL_ECHOLNPGM(")");
debug_current_and_destination((char*)"Start of ubl_line_to_destination()"); 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. * 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 // Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be. // 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(); set_current_to_destination();
if (ubl.g26_debug_flag) 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. * 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 * 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 ]), (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 * 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 // 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. // 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; float z0 = z1 + (z2 - z1) * yratio;
@ -186,20 +179,20 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"FINAL_MOVE: z_correction()"); debug_current_and_destination((char*)"FINAL_MOVE: z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); if (isnan(z0)) SERIAL_ECHO(" z0==NAN ");
if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN "); if (isnan(z_optimized)) SERIAL_ECHO(" z_optimized==NAN ");
SERIAL_ECHOPAIR(" x_end=", x_end); SERIAL_ECHOPAIR(" end[X_AXIS]=", end[X_AXIS]);
SERIAL_ECHOPAIR(" y_end=", y_end); SERIAL_ECHOPAIR(" end[Y_AXIS]=", end[Y_AXIS]);
SERIAL_ECHOPAIR(" z0=", z0); SERIAL_ECHOPAIR(" z0=", z0);
SERIAL_ECHOPAIR(" z_optimized=", z_optimized); SERIAL_ECHOPAIR(" z_optimized=", z_optimized);
SERIAL_ECHOPAIR(" err=",fabs(z_optimized - z0)); SERIAL_ECHOPAIR(" err=",fabs(z_optimized - z0));
SERIAL_EOL; 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 * If part of the Mesh is undefined, it will show up as NAN
@ -210,7 +203,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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) if (ubl.g26_debug_flag)
debug_current_and_destination((char*)"FINAL_MOVE in ubl_line_to_destination()"); debug_current_and_destination((char*)"FINAL_MOVE in ubl_line_to_destination()");
@ -227,8 +220,8 @@
* blocks of code: * blocks of code:
*/ */
const float dx = x_end - x_start, const float dx = end[X_AXIS] - start[X_AXIS],
dy = y_end - y_start; dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1 : 0, const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1 : 0; down_flag = dy < 0.0 ? 1 : 0;
@ -251,8 +244,8 @@
const bool use_x_dist = adx > ady; const bool use_x_dist = adx > ady;
float on_axis_distance = use_x_dist ? dx : dy, float on_axis_distance = use_x_dist ? dx : dy,
e_position = e_end - e_start, e_position = end[E_AXIS] - start[E_AXIS],
z_position = z_end - z_start; z_position = end[Z_AXIS] - start[Z_AXIS];
const float e_normalized_dist = e_position / on_axis_distance, const float e_normalized_dist = e_position / on_axis_distance,
z_normalized_dist = z_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; int current_xi = cell_start_xi, current_yi = cell_start_yi;
const float m = dy / dx, 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), const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance),
inf_m_flag = NEAR_ZERO(dx); 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! * else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line * 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 * 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 * 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 * 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 != y_start) { if (y != start[Y_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = y - y_start; // we don't need to check if the extruder position on_axis_distance = y - start[Y_AXIS]; // 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 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 = z_start + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
e_position = e_start; e_position = start[E_AXIS];
z_position = z_start; z_position = start[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);
@ -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. // 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; goto FINAL_MOVE;
set_current_to_destination(); set_current_to_destination();
@ -368,7 +361,7 @@
const float next_mesh_line_x = LOGICAL_X_POSITION(ubl.mesh_index_to_xpos[current_xi]), 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 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 * 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 * 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 * 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 != x_start) { if (x != start[X_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = x - x_start; // we don't need to check if the extruder position on_axis_distance = x - start[X_AXIS]; // 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 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 = z_start + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
e_position = e_start; e_position = start[E_AXIS];
z_position = z_start; z_position = start[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);
@ -427,7 +420,7 @@
if (ubl.g26_debug_flag) if (ubl.g26_debug_flag)
debug_current_and_destination((char*)"horizontal move done in ubl_line_to_destination()"); 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; goto FINAL_MOVE;
set_current_to_destination(); 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]), 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]), 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 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 x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// about m being equal to 0.0 If this was the case, we would have // (No need to worry about m being zero.
// detected this as a vertical line move up above and we wouldn't // If that was the case, it was already detected
// be down here doing a generic type of move. // as a vertical line move above.)
if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
// //
// Yes! Crossing a Y Mesh Line next // 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 * 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 * If part of the Mesh is undefined, it will show up as NAN
@ -498,13 +491,13 @@
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? x - x_start : next_mesh_line_y - y_start; on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
e_position = e_start + on_axis_distance * e_normalized_dist; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = z_start + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
e_position = e_start; e_position = start[E_AXIS];
z_position = z_start; 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); planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder);
current_yi += dyi; current_yi += dyi;
@ -514,7 +507,7 @@
// //
// Yes! Crossing a X Mesh Line next // 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 * 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 * If part of the Mesh is undefined, it will show up as NAN
@ -548,13 +541,13 @@
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? next_mesh_line_x - x_start : y - y_start; on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
e_position = e_start + on_axis_distance * e_normalized_dist; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = z_start + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
e_position = e_start; e_position = start[E_AXIS];
z_position = z_start; 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); 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) if (ubl.g26_debug_flag)
debug_current_and_destination((char*)"generic move done in ubl_line_to_destination()"); 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; goto FINAL_MOVE;
set_current_to_destination(); set_current_to_destination();