diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index 162f5a1b39..0a382c5535 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -258,8 +258,8 @@ : find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. if (location.x_index >= 0 && location.y_index >= 0) { - const float circle_x = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])), - circle_y = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index])); + const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), + circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem #ifdef DELTA @@ -401,8 +401,8 @@ for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { if (!is_bit_set(circle_flags, i, j)) { - const float mx = pgm_read_float(&(ubl.mesh_index_to_xpos[i])), // We found a circle that needs to be printed - my = pgm_read_float(&(ubl.mesh_index_to_ypos[j])); + const float mx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // We found a circle that needs to be printed + my = pgm_read_float(&ubl.mesh_index_to_ypos[j]); // Get the distance to this intersection float f = HYPOT(X - mx, Y - my); @@ -446,11 +446,11 @@ // We found two circles that need a horizontal line to connect them // Print it! // - sx = pgm_read_float(&(ubl.mesh_index_to_xpos[ i ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge - ex = pgm_read_float(&(ubl.mesh_index_to_xpos[i + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge + sx = pgm_read_float(&ubl.mesh_index_to_xpos[ i ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right edge + ex = pgm_read_float(&ubl.mesh_index_to_xpos[i + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // left edge sx = constrain(sx, X_MIN_POS + 1, X_MAX_POS - 1); - sy = ey = constrain(pgm_read_float(&(ubl.mesh_index_to_ypos[j])), Y_MIN_POS + 1, Y_MAX_POS - 1); + sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); if (ubl.g26_debug_flag) { @@ -477,10 +477,10 @@ // We found two circles that need a vertical line to connect them // Print it! // - sy = pgm_read_float(&(ubl.mesh_index_to_ypos[ j ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge - ey = pgm_read_float(&(ubl.mesh_index_to_ypos[j + 1])) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge + sy = pgm_read_float(&ubl.mesh_index_to_ypos[ j ]) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top edge + ey = pgm_read_float(&ubl.mesh_index_to_ypos[j + 1]) - (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // bottom edge - sx = ex = constrain(pgm_read_float(&(ubl.mesh_index_to_xpos[i])), X_MIN_POS + 1, X_MAX_POS - 1); + sx = ex = constrain(pgm_read_float(&ubl.mesh_index_to_xpos[i]), X_MIN_POS + 1, X_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 6515d677d9..c7a00f5952 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -4919,7 +4919,7 @@ void home_all_axes() { gcode_G28(); } // For LINEAR and 3POINT leveling correct the current position if (verbose_level > 0) - planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); + planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:")); if (!dryrun) { // @@ -6965,7 +6965,7 @@ inline void gcode_M111() { for (uint8_t i = 0; i < COUNT(debug_strings); i++) { if (TEST(marlin_debug_flags, i)) { if (comma++) SERIAL_CHAR(','); - serialprintPGM((char*)pgm_read_word(&(debug_strings[i]))); + serialprintPGM((char*)pgm_read_word(&debug_strings[i])); } } } @@ -8360,7 +8360,7 @@ void quickstop_stepper() { // V to print the matrix or mesh if (code_seen('V')) { #if ABL_PLANAR - planner.bed_level_matrix.debug("Bed Level Correction Matrix:"); + planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:")); #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) if (bilinear_grid_spacing[X_AXIS]) { print_bilinear_leveling_grid(); @@ -9545,16 +9545,16 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { - tmp_offset_vec.debug("tmp_offset_vec"); - act_offset_vec.debug("act_offset_vec"); - offset_vec.debug("offset_vec (BEFORE)"); + tmp_offset_vec.debug(PSTR("tmp_offset_vec")); + act_offset_vec.debug(PSTR("act_offset_vec")); + offset_vec.debug(PSTR("offset_vec (BEFORE)")); } #endif offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix)); #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) offset_vec.debug("offset_vec (AFTER)"); + if (DEBUGGING(LEVELING)) offset_vec.debug(PSTR("offset_vec (AFTER)")); #endif // Adjustments to the current position diff --git a/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration.h b/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration.h index cd49149ea6..1439a40cd7 100644 --- a/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration.h +++ b/Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration.h @@ -452,7 +452,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) #define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration.h index 623598ace4..9654740b11 100644 --- a/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration.h +++ b/Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration.h @@ -459,7 +459,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) //#define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/example_configurations/delta/generic/Configuration.h b/Marlin/example_configurations/delta/generic/Configuration.h index 60589f03b4..d93cd66996 100644 --- a/Marlin/example_configurations/delta/generic/Configuration.h +++ b/Marlin/example_configurations/delta/generic/Configuration.h @@ -448,7 +448,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) //#define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/example_configurations/delta/kossel_mini/Configuration.h b/Marlin/example_configurations/delta/kossel_mini/Configuration.h index 8d6c59d7e8..1e3d3ed432 100644 --- a/Marlin/example_configurations/delta/kossel_mini/Configuration.h +++ b/Marlin/example_configurations/delta/kossel_mini/Configuration.h @@ -448,7 +448,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 18) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) //#define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/example_configurations/delta/kossel_pro/Configuration.h b/Marlin/example_configurations/delta/kossel_pro/Configuration.h index 186b616e9c..91897cdfb6 100644 --- a/Marlin/example_configurations/delta/kossel_pro/Configuration.h +++ b/Marlin/example_configurations/delta/kossel_pro/Configuration.h @@ -435,7 +435,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 25.4) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) //#define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/example_configurations/delta/kossel_xl/Configuration.h b/Marlin/example_configurations/delta/kossel_xl/Configuration.h index 3e4db593f6..f3e5494704 100644 --- a/Marlin/example_configurations/delta/kossel_xl/Configuration.h +++ b/Marlin/example_configurations/delta/kossel_xl/Configuration.h @@ -453,7 +453,7 @@ // set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm - + // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) //#define DELTA_AUTO_CALIBRATION #if ENABLED(DELTA_AUTO_CALIBRATION) diff --git a/Marlin/least_squares_fit.cpp b/Marlin/least_squares_fit.cpp index ce21b3a053..a6fb3c4457 100644 --- a/Marlin/least_squares_fit.cpp +++ b/Marlin/least_squares_fit.cpp @@ -66,12 +66,12 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) { lsf->xbar /= N; lsf->ybar /= N; lsf->zbar /= N; - lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar; - lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar; - lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar; - lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar; - lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar; - lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar; + lsf->x2bar = lsf->x2bar / N - sq(lsf->xbar); + lsf->y2bar = lsf->y2bar / N - sq(lsf->ybar); + lsf->z2bar = lsf->z2bar / N - sq(lsf->zbar); + lsf->xybar = lsf->xybar / N - sq(lsf->xbar); + lsf->yzbar = lsf->yzbar / N - sq(lsf->ybar); + lsf->xzbar = lsf->xzbar / N - sq(lsf->xbar); const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar); if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy)) diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp index b2c4a7ef41..e9f97b4adc 100644 --- a/Marlin/ubl_G29.cpp +++ b/Marlin/ubl_G29.cpp @@ -31,10 +31,14 @@ #include "hex_print_routines.h" #include "configuration_store.h" #include "ultralcd.h" + #include "stepper.h" #include #include "least_squares_fit.h" + extern float destination[XYZE]; + extern float current_position[XYZE]; + void lcd_return_to_status(); bool lcd_clicked(); void lcd_implementation_clear(); @@ -317,6 +321,7 @@ void __attribute__((optimize("O0"))) gcode_G29() { + if (ubl.eeprom_start < 0) { SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it"); SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n"); @@ -347,7 +352,6 @@ } if (code_seen('Q')) { - const int test_pattern = code_has_value() ? code_value_int() : -1; if (!WITHIN(test_pattern, 0, 2)) { SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n"); @@ -428,15 +432,16 @@ // SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n"); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); - if (!x_flag && !y_flag) { // use a good default location for the path - // The flipped > and < operators on these two comparisons is - // intentional. It should cause the probed points to follow a - // nice path on Cartesian printers. It may make sense to - // have Delta printers default to the center of the bed. - // For now, until that is decided, it can be forced with the X - // and Y parameters. - x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS; - y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS; + if (!x_flag && !y_flag) { + /** + * Use a good default location for the path. + * The flipped > and < operators in these comparisons is intentional. + * It should cause the probed points to follow a nice path on Cartesian printers. + * It may make sense to have Delta printers default to the center of the bed. + * Until that is decided, this can be forced with the X and Y parameters. + */ + x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? UBL_MESH_MAX_X : UBL_MESH_MIN_X; + y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? UBL_MESH_MAX_Y : UBL_MESH_MIN_Y; } if (code_seen('C')) { @@ -455,27 +460,29 @@ } } manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M')); + SERIAL_PROTOCOLLNPGM("G29 P2 finished"); } break; case 3: { - // - // Populate invalid Mesh areas. Two choices are available to the user. The user can - // specify the constant to be used with a C # paramter. Or the user can allow the G29 P3 command to - // apply a 'reasonable' constant to the invalid mesh point. Some caution and scrutiny should be used - // on either of these paths! - // + /** + * Populate invalid mesh areas. Proceed with caution. + * Two choices are available: + * - Specify a constant with the 'C' parameter. + * - Allow 'G29 P3' to choose a 'reasonable' constant. + */ if (c_flag) { - while (repetition_cnt--) { + while (repetition_cnt--) { const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); - if (location.x_index < 0) break; // No more invalid Mesh Points to populate - ubl.z_values[location.x_index][location.y_index] = ubl_constant; + if (location.x_index < 0) break; // No more invalid Mesh Points to populate + ubl.z_values[location.x_index][location.y_index] = ubl_constant; } break; - } else // The user wants to do a 'Smart' fill where we use the surrounding known - smart_fill_mesh(); // values to provide a good guess of what the unprobed mesh point should be - break; } + else + smart_fill_mesh(); // Do a 'Smart' fill using nearby known values + + } break; case 4: // @@ -483,55 +490,19 @@ // fine_tune_mesh(x_pos, y_pos, code_seen('O') || code_seen('M')); break; - case 5: - ubl.find_mean_mesh_height(); - break; - case 6: - ubl.shift_mesh_height(); - break; - case 10: - // [DEBUG] Pay no attention to this stuff. It can be removed soon. - SERIAL_ECHO_START; - SERIAL_ECHOLNPGM("Checking G29 has control of LCD Panel:"); - KEEPALIVE_STATE(PAUSED_FOR_USER); - ubl.has_control_of_lcd_panel = true; - while (!ubl_lcd_clicked()) { - safe_delay(250); - if (ubl.encoder_diff) { - SERIAL_ECHOLN((int)ubl.encoder_diff); - ubl.encoder_diff = 0; - } - } - SERIAL_ECHOLNPGM("G29 giving back control of LCD Panel."); - ubl.has_control_of_lcd_panel = false; - KEEPALIVE_STATE(IN_HANDLER); - break; + case 5: ubl.find_mean_mesh_height(); break; - case 11: - // [DEBUG] wait_for_user code. Pay no attention to this stuff. It can be removed soon. - SERIAL_ECHO_START; - SERIAL_ECHOLNPGM("Checking G29 has control of LCD Panel:"); - KEEPALIVE_STATE(PAUSED_FOR_USER); - wait_for_user = true; - while (wait_for_user) { - safe_delay(250); - if (ubl.encoder_diff) { - SERIAL_ECHOLN((int)ubl.encoder_diff); - ubl.encoder_diff = 0; - } - } - SERIAL_ECHOLNPGM("G29 giving back control of LCD Panel."); - KEEPALIVE_STATE(IN_HANDLER); - break; + case 6: ubl.shift_mesh_height(); break; } + } if (code_seen('T')) { - float z1 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level), - z2 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level), - z3 = probe_pt( LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level); + float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level), + z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level), + z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, 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 // the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is) @@ -541,7 +512,7 @@ z2 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ; z3 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ; - 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(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y))); ubl.tilt_mesh_based_on_3pts(z1, z2, z3); ubl.restore_ubl_active_state_and_leave(); } @@ -600,8 +571,8 @@ SERIAL_ECHOPAIR(" J ", y); SERIAL_ECHOPGM(" Z "); SERIAL_ECHO_F(ubl.z_values[x][y], 6); - SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[x])))); - SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[y])))); + SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[x]))); + SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[y]))); SERIAL_EOL; } return; @@ -647,9 +618,9 @@ } while (!ubl_lcd_clicked()); ubl.has_control_of_lcd_panel = true; // There is a race condition for the Encoder Wheel getting clicked. - // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune) - // or here. So, until we are done looking for a long Encoder Wheel Press, - // we need to take control of the panel + // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune) + // or here. So, until we are done looking for a long Encoder Wheel Press, + // we need to take control of the panel KEEPALIVE_STATE(IN_HANDLER); @@ -686,44 +657,39 @@ } void unified_bed_leveling::find_mean_mesh_height() { - uint8_t x, y; - int n; - float sum, sum_of_diff_squared, sigma, difference, mean; - - sum = sum_of_diff_squared = 0.0; - n = 0; - for (x = 0; x < GRID_MAX_POINTS_X; x++) - for (y = 0; y < GRID_MAX_POINTS_Y; y++) + float sum = 0.0; + int n = 0; + for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) + for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) if (!isnan(ubl.z_values[x][y])) { sum += ubl.z_values[x][y]; n++; } - mean = sum / n; + const float mean = sum / n; // // Now do the sumation of the squares of difference from mean // - for (x = 0; x < GRID_MAX_POINTS_X; x++) - for (y = 0; y < GRID_MAX_POINTS_Y; y++) - if (!isnan(ubl.z_values[x][y])) { - difference = (ubl.z_values[x][y] - mean); - sum_of_diff_squared += difference * difference; - } + float sum_of_diff_squared = 0.0; + for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) + for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) + if (!isnan(ubl.z_values[x][y])) + sum_of_diff_squared += sq(ubl.z_values[x][y] - mean); SERIAL_ECHOLNPAIR("# of samples: ", n); SERIAL_ECHOPGM("Mean Mesh Height: "); SERIAL_ECHO_F(mean, 6); SERIAL_EOL; - sigma = sqrt(sum_of_diff_squared / (n + 1)); + const float sigma = sqrt(sum_of_diff_squared / (n + 1)); SERIAL_ECHOPGM("Standard Deviation: "); SERIAL_ECHO_F(sigma, 6); SERIAL_EOL; if (c_flag) - for (x = 0; x < GRID_MAX_POINTS_X; x++) - for (y = 0; y < GRID_MAX_POINTS_Y; y++) + for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) + for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) if (!isnan(ubl.z_values[x][y])) ubl.z_values[x][y] -= mean + ubl_constant; } @@ -761,8 +727,8 @@ location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, do_furthest); if (location.x_index >= 0 && location.y_index >= 0) { - const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])), - rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index])); + const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), + rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) if (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) { @@ -785,13 +751,12 @@ ubl.restore_ubl_active_state_and_leave(); do_blocking_move_to_xy( - constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS), - constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS) + constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X), + constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y) ); } void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) { - float d, t, inv_z; int i, j; matrix_3x3 rotation; @@ -812,94 +777,96 @@ * However, we don't know its direction. We need it to point up. So if * Z is negative, we need to invert the sign of all components of the vector */ - if ( normal.z < 0.0 ) { + if (normal.z < 0.0) { normal.x = -normal.x; normal.y = -normal.y; normal.z = -normal.z; } - rotation = matrix_3x3::create_look_at( vector_3( normal.x, normal.y, 1)); + rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1)); - if (g29_verbose_level>2) { + if (g29_verbose_level > 2) { SERIAL_ECHOPGM("bed plane normal = ["); - SERIAL_PROTOCOL_F( normal.x, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.y, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.z, 7); - SERIAL_ECHOPGM("]\n"); - rotation.debug("rotation matrix:"); + SERIAL_PROTOCOL_F(normal.x, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.y, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.z, 7); + SERIAL_ECHOLNPGM("]"); + rotation.debug(PSTR("rotation matrix:")); } // // All of 3 of these points should give us the same d constant // - t = normal.x * UBL_PROBE_PT_1_X + normal.y * UBL_PROBE_PT_1_Y; - d = t + normal.z * z1; + float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y), + d = t + normal.z * z1; if (g29_verbose_level>2) { SERIAL_ECHOPGM("D constant: "); - SERIAL_PROTOCOL_F( d, 7); - SERIAL_ECHOPGM(" \n"); + SERIAL_PROTOCOL_F(d, 7); + SERIAL_ECHOLNPGM(" "); } #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPGM("d from 1st point: "); - SERIAL_ECHO_F(d, 6); - SERIAL_EOL; - t = normal.x * UBL_PROBE_PT_2_X + normal.y * UBL_PROBE_PT_2_Y; - d = t + normal.z * z2; - SERIAL_ECHOPGM("d from 2nd point: "); - SERIAL_ECHO_F(d, 6); - SERIAL_EOL; - t = normal.x * UBL_PROBE_PT_3_X + normal.y * UBL_PROBE_PT_3_Y; - d = t + normal.z * z3; - SERIAL_ECHOPGM("d from 3rd point: "); - SERIAL_ECHO_F(d, 6); - SERIAL_EOL; + SERIAL_ECHOPGM("d from 1st point: "); + SERIAL_ECHO_F(d, 6); + SERIAL_EOL; + t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y); + d = t + normal.z * z2; + SERIAL_ECHOPGM("d from 2nd point: "); + SERIAL_ECHO_F(d, 6); + SERIAL_EOL; + t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y); + d = t + normal.z * z3; + SERIAL_ECHOPGM("d from 3rd point: "); + SERIAL_ECHO_F(d, 6); + SERIAL_EOL; } #endif - for (i = 0; i < GRID_MAX_POINTS_X; i++) { - for (j = 0; j < GRID_MAX_POINTS_Y; j++) { - float x_tmp, y_tmp, z_tmp; - x_tmp = pgm_read_float(ubl.mesh_index_to_xpos[i]); - y_tmp = pgm_read_float(ubl.mesh_index_to_ypos[j]); - z_tmp = ubl.z_values[i][j]; - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPGM("before rotation = ["); - SERIAL_PROTOCOL_F( x_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( y_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( z_tmp, 7); - SERIAL_ECHOPGM("] ---> "); - safe_delay(20); + for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { + for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { + float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]), + y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]), + z_tmp = ubl.z_values[i][j]; + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPGM("before rotation = ["); + SERIAL_PROTOCOL_F(x_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(y_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(z_tmp, 7); + SERIAL_ECHOPGM("] ---> "); + safe_delay(20); + } + #endif + apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp); + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPGM("after rotation = ["); + SERIAL_PROTOCOL_F(x_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(y_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(z_tmp, 7); + SERIAL_ECHOLNPGM("]"); + safe_delay(55); + } + #endif + ubl.z_values[i][j] += z_tmp - d; } - #endif - apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp); - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPGM("after rotation = ["); - SERIAL_PROTOCOL_F( x_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( y_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( z_tmp, 7); - SERIAL_ECHOPGM("]\n"); - safe_delay(55); } - #endif - ubl.z_values[i][j] += z_tmp - d; - } - } - return; } float use_encoder_wheel_to_measure_point() { + + while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel + delay(50); // debounce + KEEPALIVE_STATE(PAUSED_FOR_USER); while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! idle(); @@ -912,21 +879,29 @@ return current_position[Z_AXIS]; } + static void say_and_take_a_measurement() { + SERIAL_PROTOCOLLNPGM(" and take a measurement."); + } + float measure_business_card_thickness(const float &in_height) { - ubl.has_control_of_lcd_panel = true; - ubl.save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe + ubl.save_ubl_active_state_and_disable(); // Disable bed level correction for probing - SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement."); do_blocking_move_to_z(in_height); - do_blocking_move_to_xy((float(X_MAX_POS) - float(X_MIN_POS)) / 2.0, (float(Y_MAX_POS) - float(Y_MIN_POS)) / 2.0); - //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0); + do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y))); + //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]) / 2.0); + + stepper.synchronize(); + SERIAL_PROTOCOLPGM("Place shim under nozzle"); + say_and_take_a_measurement(); const float z1 = use_encoder_wheel_to_measure_point(); do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); - ubl.has_control_of_lcd_panel = false; + stepper.synchronize(); + + SERIAL_PROTOCOLPGM("Remove shim"); + say_and_take_a_measurement(); - SERIAL_PROTOCOLLNPGM("Remove Shim and Measure Bed Height."); const float z2 = use_encoder_wheel_to_measure_point(); do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); @@ -935,6 +910,8 @@ SERIAL_PROTOCOL_F(abs(z1 - z2), 6); SERIAL_PROTOCOLLNPGM("mm thick."); } + ubl.has_control_of_lcd_panel = false; + ubl.restore_ubl_active_state_and_leave(); return abs(z1 - z2); } @@ -953,11 +930,11 @@ // It doesn't matter if the probe can't reach the NAN location. This is a manual probe. if (location.x_index < 0 && location.y_index < 0) continue; - const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])), - rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index])); + const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), + rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) - if (!WITHIN(rawx, X_MIN_POS, X_MAX_POS) || !WITHIN(rawy, Y_MIN_POS, Y_MAX_POS)) { + if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { SERIAL_ERROR_START; SERIAL_ERRORLNPGM("Attempt to probe off the bed."); ubl.has_control_of_lcd_panel = false; @@ -984,7 +961,9 @@ if (do_ubl_mesh_map) ubl.display_map(map_type); // show user where we're probing - while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! + while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel + delay(50); // debounce + while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! idle(); if (ubl.encoder_diff) { do_blocking_move_to_z(current_position[Z_AXIS] + float(ubl.encoder_diff) / 100.0); @@ -1024,17 +1003,28 @@ do_blocking_move_to_xy(lx, ly); } + static void say_ubl_name() { + SERIAL_PROTOCOLPGM("Unified Bed Leveling "); + } + + static void report_ubl_state() { + say_ubl_name(); + SERIAL_PROTOCOLPGM("System "); + if (!ubl.state.active) SERIAL_PROTOCOLPGM("de"); + SERIAL_PROTOCOLLNPGM("activated.\n"); + } + bool g29_parameter_parsing() { bool err_flag = false; - LCD_MESSAGEPGM("Doing G29 UBL!"); + LCD_MESSAGEPGM("Doing G29 UBL!"); + lcd_quick_feedback(); + ubl_constant = 0.0; repetition_cnt = 0; - lcd_quick_feedback(); x_flag = code_seen('X') && code_has_value(); x_pos = x_flag ? code_value_float() : current_position[X_AXIS]; - y_flag = code_seen('Y') && code_has_value(); y_pos = y_flag ? code_value_float() : current_position[Y_AXIS]; @@ -1042,14 +1032,14 @@ if (repeat_flag) { repetition_cnt = code_has_value() ? code_value_int() : (GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y); if (repetition_cnt < 1) { - SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n"); + SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n"); return UBL_ERR; } } g29_verbose_level = code_seen('V') ? code_value_int() : 0; if (!WITHIN(g29_verbose_level, 0, 4)) { - SERIAL_PROTOCOLLNPGM("Invalid Verbose Level specified. (0-4)\n"); + SERIAL_PROTOCOLLNPGM("?(V)erbose Level is implausible (0-4)\n"); err_flag = true; } @@ -1066,44 +1056,47 @@ err_flag = true; } - if (!WITHIN(RAW_X_POSITION(x_pos), X_MIN_POS, X_MAX_POS)) { + if (!WITHIN(RAW_X_POSITION(x_pos), UBL_MESH_MIN_X, UBL_MESH_MAX_X)) { SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n"); err_flag = true; } - if (!WITHIN(RAW_Y_POSITION(y_pos), Y_MIN_POS, Y_MAX_POS)) { + if (!WITHIN(RAW_Y_POSITION(y_pos), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n"); err_flag = true; } if (err_flag) return UBL_ERR; - if (code_seen('A')) { // Activate the Unified Bed Leveling System + // Activate or deactivate UBL + if (code_seen('A')) { + if (code_seen('D')) { + SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n"); + return UBL_ERR; + } ubl.state.active = 1; - SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System activated.\n"); + report_ubl_state(); } - - c_flag = code_seen('C'); - if (c_flag) - ubl_constant = code_value_float(); - - if (code_seen('D')) { // Disable the Unified Bed Leveling System + else if (code_seen('D')) { ubl.state.active = 0; - SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System de-activated.\n"); + report_ubl_state(); } + // Set global 'C' flag and its value + if ((c_flag = code_seen('C'))) + ubl_constant = code_value_float(); + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (code_seen('F') && code_has_value()) { const float fh = code_value_float(); if (!WITHIN(fh, 0.0, 100.0)) { - SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n"); + SERIAL_PROTOCOLLNPGM("?(F)ade height for Bed Level Correction not plausible.\n"); return UBL_ERR; } set_z_fade_height(fh); } #endif - map_type = code_seen('O') && code_has_value() ? code_value_int() : 0; if (!WITHIN(map_type, 0, 1)) { SERIAL_PROTOCOLLNPGM("Invalid map type.\n"); @@ -1125,7 +1118,7 @@ * This function goes away after G29 debug is complete. But for right now, it is a handy * routine to dump binary data structures. */ -/* + /* void dump(char * const str, const float &f) { char *ptr; @@ -1143,7 +1136,7 @@ SERIAL_EOL; } -*/ + //*/ static int ubl_state_at_invocation = 0, ubl_state_recursion_chk = 0; @@ -1170,7 +1163,6 @@ ubl.state.active = ubl_state_at_invocation; } - /** * Much of the 'What?' command can be eliminated. But until we are fully debugged, it is * good to have the extra information. Soon... we prune this to just a few items @@ -1178,7 +1170,8 @@ void g29_what_command() { const uint16_t k = E2END - ubl.eeprom_start; - SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version " UBL_VERSION " "); + say_ubl_name(); + SERIAL_PROTOCOLPGM("System Version " UBL_VERSION " "); if (ubl.state.active) SERIAL_PROTOCOLCHAR('A'); else @@ -1209,11 +1202,11 @@ SERIAL_EOL; safe_delay(25); - SERIAL_PROTOCOLLNPAIR("ubl.eeprom_start=0x", hex_word(ubl.eeprom_start)); + SERIAL_PROTOCOLLNPAIR("ubl.eeprom_start=", hex_address((void*)ubl.eeprom_start)); SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: "); for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { - SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(pgm_read_float(&(ubl.mesh_index_to_xpos[i]))), 1); + SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[i])), 1); SERIAL_PROTOCOLPGM(" "); safe_delay(50); } @@ -1221,7 +1214,7 @@ SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) { - SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(pgm_read_float(&(ubl.mesh_index_to_ypos[i]))), 1); + SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[i])), 1); SERIAL_PROTOCOLPGM(" "); safe_delay(50); } @@ -1275,8 +1268,10 @@ SERIAL_EOL; safe_delay(50); - if (!ubl.sanity_check()) - SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed."); + if (!ubl.sanity_check()) { + say_ubl_name(); + SERIAL_PROTOCOLLNPGM("sanity checks passed."); + } } /** @@ -1336,18 +1331,18 @@ ubl.z_values[x][y] -= tmp_z_values[x][y]; } - mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], bool far_flag) { - float distance, closest = far_flag ? -99999.99 : 99999.99; - mesh_index_pair return_val; - - return_val.x_index = return_val.y_index = -1; + mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], const bool far_flag) { + mesh_index_pair out_mesh; + out_mesh.x_index = out_mesh.y_index = -1; const float current_x = current_position[X_AXIS], current_y = current_position[Y_AXIS]; // Get our reference position. Either the nozzle or probe location. - const float px = lx - (probe_as_reference==USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0), - py = ly - (probe_as_reference==USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0); + const float px = lx - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0), + py = ly - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0); + + float closest = far_flag ? -99999.99 : 99999.99; for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { @@ -1359,13 +1354,13 @@ // We only get here if we found a Mesh Point of the specified type - const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[i])), // Check if we can probe this mesh location - rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[j])); + const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // Check if we can probe this mesh location + rawy = pgm_read_float(&ubl.mesh_index_to_ypos[j]); // If using the probe as the reference there are some unreachable locations. // Prune them from the list and ignore them till the next Phase (manual nozzle probing). - if (probe_as_reference==USE_PROBE_AS_REFERENCE && + if (probe_as_reference == USE_PROBE_AS_REFERENCE && (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) ) continue; @@ -1375,30 +1370,38 @@ const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location my = LOGICAL_Y_POSITION(rawy); - distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.1; + float distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.1; - if (far_flag) { // If doing the far_flag action, we want to be as far as possible - for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) { // from the starting point and from any other probed points. We - for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) { // want the next point spread out and filling in any blank spaces - if (!isnan(ubl.z_values[k][l])) { // in the mesh. So we add in some of the distance to every probed - distance += sq(i - k) * (MESH_X_DIST) * .05 // point we can find. + /** + * If doing the far_flag action, we want to be as far as possible + * from the starting point and from any other probed points. We + * want the next point spread out and filling in any blank spaces + * in the mesh. So we add in some of the distance to every probed + * point we can find. + */ + if (far_flag) { + for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) { + for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) { + if (!isnan(ubl.z_values[k][l])) { + distance += sq(i - k) * (MESH_X_DIST) * .05 + sq(j - l) * (MESH_Y_DIST) * .05; } } } } - if (far_flag == (distance > closest) && distance != closest) { // if far_flag, look for farthest point + // if far_flag, look for farthest point + if (far_flag == (distance > closest) && distance != closest) { closest = distance; // We found a closer/farther location with - return_val.x_index = i; // the specified type of mesh value. - return_val.y_index = j; - return_val.distance = closest; + out_mesh.x_index = i; // the specified type of mesh value. + out_mesh.y_index = j; + out_mesh.distance = closest; } } } // for j } // for i - return return_val; + return out_mesh; } void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) { @@ -1418,50 +1421,55 @@ do_blocking_move_to_xy(lx, ly); do { location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false); - // It doesn't matter if the probe can not reach this - // location. This is a manual edit of the Mesh Point. + // It doesn't matter if the probe can't reach this + // location. This is a manual edit of the Mesh Point. if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points. bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a // different location the next time through the loop - const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_index])), - rawy = pgm_read_float(&(ubl.mesh_index_to_ypos[location.y_index])); + const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), + rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) - if (!WITHIN(rawx, X_MIN_POS, X_MAX_POS) || !WITHIN(rawy, Y_MIN_POS, Y_MAX_POS)) { // In theory, we don't need this check. + if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { // In theory, we don't need this check. SERIAL_ERROR_START; SERIAL_ERRORLNPGM("Attempt to edit off the bed."); // This really can't happen, but do the check for now ubl.has_control_of_lcd_panel = false; goto FINE_TUNE_EXIT; } - do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit - do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy)); - float new_z = ubl.z_values[location.x_index][location.y_index]; - round_off = (int32_t)(new_z * 1000.0); // we chop off the last digits just to be clean. We are rounding to the - new_z = float(round_off) / 1000.0; + if (!isnan(new_z)) { //can't fine tune a point that hasn't been probed - KEEPALIVE_STATE(PAUSED_FOR_USER); - ubl.has_control_of_lcd_panel = true; + do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit + do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy)); - if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted + round_off = (int32_t)(new_z * 1000.0); // we chop off the last digits just to be clean. We are rounding to the + new_z = float(round_off) / 1000.0; - lcd_implementation_clear(); - lcd_mesh_edit_setup(new_z); + KEEPALIVE_STATE(PAUSED_FOR_USER); + ubl.has_control_of_lcd_panel = true; - do { - new_z = lcd_mesh_edit(); - idle(); - } while (!ubl_lcd_clicked()); + if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted - lcd_return_to_status(); + lcd_implementation_clear(); - ubl.has_control_of_lcd_panel = true; // There is a race condition for the Encoder Wheel getting clicked. - // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune) - // or here. + lcd_mesh_edit_setup(new_z); + + do { + new_z = lcd_mesh_edit(); + idle(); + } while (!ubl_lcd_clicked()); + + lcd_return_to_status(); + + // There is a race condition for the Encoder Wheel getting clicked. + // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune) + // or here. + ubl.has_control_of_lcd_panel = true; + } const millis_t nxt = millis() + 1500UL; while (ubl_lcd_clicked()) { // debounce and watch for abort @@ -1501,229 +1509,193 @@ SERIAL_ECHOLNPGM("Done Editing Mesh"); } - // - // The routine provides the 'Smart Fill' capability. It scans from the - // outward edges of the mesh towards the center. If it finds an invalid - // location, it uses the next two points (assumming they are valid) to - // calculate a 'reasonable' value for the unprobed mesh point. - // - void smart_fill_mesh() { - float f, diff; - for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Bottom of the mesh looking up - for (uint8_t y = 0; y < GRID_MAX_POINTS_Y-2; y++) { - if (isnan(ubl.z_values[x][y])) { - if (isnan(ubl.z_values[x][y+1])) // we only deal with the first NAN next to a block of - continue; // good numbers. we want 2 good numbers to extrapolate off of. - if (isnan(ubl.z_values[x][y+2])) - continue; - if (ubl.z_values[x][y+1] < ubl.z_values[x][y+2]) // The bed is angled down near this edge. So to be safe, we - ubl.z_values[x][y] = ubl.z_values[x][y+1]; // use the closest value, which is probably a little too high - else { - diff = ubl.z_values[x][y+1] - ubl.z_values[x][y+2]; // The bed is angled up near this edge. So we will use the closest - ubl.z_values[x][y] = ubl.z_values[x][y+1] + diff; // height and add in the difference between that and the next point - } - break; - } + /** + * 'Smart Fill': Scan from the outward edges of the mesh towards the center. + * If an invalid location is found, use the next two points (if valid) to + * calculate a 'reasonable' value for the unprobed mesh point. + */ + + bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) { + const int8_t x1 = x + xdir, x2 = x1 + xdir, + y1 = y + ydir, y2 = y1 + ydir; + // A NAN next to a pair of real values? + if (isnan(ubl.z_values[x][y]) && !isnan(ubl.z_values[x1][y1]) && !isnan(ubl.z_values[x2][y2])) { + if (ubl.z_values[x1][y1] < ubl.z_values[x2][y2]) // Angled downward? + ubl.z_values[x][y] = ubl.z_values[x1][y1]; // Use nearest (maybe a little too high.) + else { + const float diff = ubl.z_values[x1][y1] - ubl.z_values[x2][y2]; // Angled upward + ubl.z_values[x][y] = ubl.z_values[x1][y1] + diff; // Use closest plus difference } + return true; } - for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Top of the mesh looking down - for (uint8_t y=GRID_MAX_POINTS_Y-1; y>=1; y--) { - if (isnan(ubl.z_values[x][y])) { - if (isnan(ubl.z_values[x][y-1])) // we only deal with the first NAN next to a block of - continue; // good numbers. we want 2 good numbers to extrapolate off of. - if (isnan(ubl.z_values[x][y-2])) - continue; - if (ubl.z_values[x][y-1] < ubl.z_values[x][y-2]) // The bed is angled down near this edge. So to be safe, we - ubl.z_values[x][y] = ubl.z_values[x][y-1]; // use the closest value, which is probably a little too high - else { - diff = ubl.z_values[x][y-1] - ubl.z_values[x][y-2]; // The bed is angled up near this edge. So we will use the closest - ubl.z_values[x][y] = ubl.z_values[x][y-1] + diff; // height and add in the difference between that and the next point - } - break; - } - } + return false; + } + + typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info; + + void smart_fill_loop(const smart_fill_info &f) { + if (f.yfirst) { + const int8_t dir = f.ex > f.sx ? 1 : -1; + for (uint8_t y = f.sy; y != f.ey; ++y) + for (uint8_t x = f.sx; x != f.ex; x += dir) + if (smart_fill_one(x, y, dir, 0)) break; } - for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { - for (uint8_t x = 0; x < GRID_MAX_POINTS_X-2; x++) { // Left side of the mesh looking right - if (isnan(ubl.z_values[x][y])) { - if (isnan(ubl.z_values[x+1][y])) // we only deal with the first NAN next to a block of - continue; // good numbers. we want 2 good numbers to extrapolate off of. - if (isnan(ubl.z_values[x+2][y])) - continue; - if (ubl.z_values[x+1][y] < ubl.z_values[x+2][y]) // The bed is angled down near this edge. So to be safe, we - ubl.z_values[x][y] = ubl.z_values[x][y+1]; // use the closest value, which is probably a little too high - else { - diff = ubl.z_values[x+1][y] - ubl.z_values[x+2][y]; // The bed is angled up near this edge. So we will use the closest - ubl.z_values[x][y] = ubl.z_values[x+1][y] + diff; // height and add in the difference between that and the next point - } - break; - } - } - } - for (uint8_t y=0; y < GRID_MAX_POINTS_Y; y++) { - for (uint8_t x=GRID_MAX_POINTS_X-1; x>=1; x--) { // Right side of the mesh looking left - if (isnan(ubl.z_values[x][y])) { - if (isnan(ubl.z_values[x-1][y])) // we only deal with the first NAN next to a block of - continue; // good numbers. we want 2 good numbers to extrapolate off of. - if (isnan(ubl.z_values[x-2][y])) - continue; - if (ubl.z_values[x-1][y] < ubl.z_values[x-2][y]) // The bed is angled down near this edge. So to be safe, we - ubl.z_values[x][y] = ubl.z_values[x-1][y]; // use the closest value, which is probably a little too high - else { - diff = ubl.z_values[x-1][y] - ubl.z_values[x-2][y]; // The bed is angled up near this edge. So we will use the closest - ubl.z_values[x][y] = ubl.z_values[x-1][y] + diff; // height and add in the difference between that and the next point - } - break; - } - } + else { + const int8_t dir = f.ey > f.sy ? 1 : -1; + for (uint8_t x = f.sx; x != f.ex; ++x) + for (uint8_t y = f.sy; y != f.ey; y += dir) + if (smart_fill_one(x, y, 0, dir)) break; } } + void smart_fill_mesh() { + const smart_fill_info info[] = { + { 0, GRID_MAX_POINTS_X, 0, GRID_MAX_POINTS_Y - 2, false }, // Bottom of the mesh looking up + { 0, GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y - 1, 0, false }, // Top of the mesh looking down + { 0, GRID_MAX_POINTS_X - 2, 0, GRID_MAX_POINTS_Y, true }, // Left side of the mesh looking right + { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true } // Right side of the mesh looking left + }; + for (uint8_t i = 0; i < COUNT(info); ++i) smart_fill_loop(info[i]); + } void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) { - int8_t i, j ,k, xCount, yCount, xi, yi; // counter variables - int8_t ix, iy, zig_zag=0, status; + constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X), + x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X), + y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y), + y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y); + + const float dx = float(x_max - x_min) / (grid_size - 1.0), + dy = float(y_max - y_min) / (grid_size - 1.0); - float dx, dy, x, y, measured_z, inv_z; struct linear_fit_data lsf_results; - matrix_3x3 rotation; - vector_3 normal; - - int16_t x_min = max((MIN_PROBE_X),(UBL_MESH_MIN_X)), - x_max = min((MAX_PROBE_X),(UBL_MESH_MAX_X)), - y_min = max((MIN_PROBE_Y),(UBL_MESH_MIN_Y)), - y_max = min((MAX_PROBE_Y),(UBL_MESH_MAX_Y)); - - dx = ((float)(x_max-x_min)) / (grid_size-1.0); - dy = ((float)(y_max-y_min)) / (grid_size-1.0); - incremental_LSF_reset(&lsf_results); - for(ix=0; ix>>---> "); - SERIAL_PROTOCOL_F( measured_z, 7); - SERIAL_ECHOPGM("\n"); - } - #endif - incremental_LSF(&lsf_results, x, y, measured_z); + measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ; + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPGM(" final >>>---> "); + SERIAL_PROTOCOL_F(measured_z, 7); + SERIAL_EOL; + } + #endif + + incremental_LSF(&lsf_results, x, y, measured_z); } - zig_zag = !zig_zag; - } + zig_zag ^= true; + } - status = finish_incremental_LSF(&lsf_results); - if (g29_verbose_level>3) { + const int status = finish_incremental_LSF(&lsf_results); + + if (g29_verbose_level > 3) { SERIAL_ECHOPGM("LSF Results A="); - SERIAL_PROTOCOL_F( lsf_results.A, 7); + SERIAL_PROTOCOL_F(lsf_results.A, 7); SERIAL_ECHOPGM(" B="); - SERIAL_PROTOCOL_F( lsf_results.B, 7); + SERIAL_PROTOCOL_F(lsf_results.B, 7); SERIAL_ECHOPGM(" D="); - SERIAL_PROTOCOL_F( lsf_results.D, 7); - SERIAL_CHAR('\n'); - } + SERIAL_PROTOCOL_F(lsf_results.D, 7); + SERIAL_EOL; + } - normal = vector_3( lsf_results.A, lsf_results.B, 1.0000); - normal = normal.get_normal(); + vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal(); - if (g29_verbose_level>2) { + if (g29_verbose_level > 2) { SERIAL_ECHOPGM("bed plane normal = ["); - SERIAL_PROTOCOL_F( normal.x, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.y, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.z, 7); - SERIAL_ECHOPGM("]\n"); + SERIAL_PROTOCOL_F(normal.x, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.y, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.z, 7); + SERIAL_ECHOLNPGM("]"); + } + + matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1)); + + for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { + for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { + float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]), + y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]), + z_tmp = ubl.z_values[i][j]; + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPGM("before rotation = ["); + SERIAL_PROTOCOL_F(x_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(y_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(z_tmp, 7); + SERIAL_ECHOPGM("] ---> "); + safe_delay(20); + } + #endif + + apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp); + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPGM("after rotation = ["); + SERIAL_PROTOCOL_F(x_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(y_tmp, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(z_tmp, 7); + SERIAL_ECHOLNPGM("]"); + safe_delay(55); + } + #endif + + ubl.z_values[i][j] += z_tmp - lsf_results.D; } - - rotation = matrix_3x3::create_look_at( vector_3( lsf_results.A, lsf_results.B, 1)); - - for (i = 0; i < GRID_MAX_POINTS_X; i++) { - for (j = 0; j < GRID_MAX_POINTS_Y; j++) { - float x_tmp, y_tmp, z_tmp; - x_tmp = pgm_read_float(&(ubl.mesh_index_to_xpos[i])); - y_tmp = pgm_read_float(&(ubl.mesh_index_to_ypos[j])); - z_tmp = ubl.z_values[i][j]; - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPGM("before rotation = ["); - SERIAL_PROTOCOL_F( x_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( y_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( z_tmp, 7); - SERIAL_ECHOPGM("] ---> "); - safe_delay(20); - } - #endif - apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp); - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPGM("after rotation = ["); - SERIAL_PROTOCOL_F( x_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( y_tmp, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( z_tmp, 7); - SERIAL_ECHOPGM("]\n"); - safe_delay(55); - } - - #endif - - ubl.z_values[i][j] += z_tmp - lsf_results.D; - } - } + } #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { - rotation.debug("rotation matrix:"); + rotation.debug(PSTR("rotation matrix:")); SERIAL_ECHOPGM("LSF Results A="); - SERIAL_PROTOCOL_F( lsf_results.A, 7); + SERIAL_PROTOCOL_F(lsf_results.A, 7); SERIAL_ECHOPGM(" B="); - SERIAL_PROTOCOL_F( lsf_results.B, 7); + SERIAL_PROTOCOL_F(lsf_results.B, 7); SERIAL_ECHOPGM(" D="); - SERIAL_PROTOCOL_F( lsf_results.D, 7); - SERIAL_CHAR('\n'); + SERIAL_PROTOCOL_F(lsf_results.D, 7); + SERIAL_EOL; safe_delay(55); SERIAL_ECHOPGM("bed plane normal = ["); - SERIAL_PROTOCOL_F( normal.x, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.y, 7); - SERIAL_ECHOPGM(","); - SERIAL_PROTOCOL_F( normal.z, 7); + SERIAL_PROTOCOL_F(normal.x, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.y, 7); + SERIAL_PROTOCOLCHAR(','); + SERIAL_PROTOCOL_F(normal.z, 7); SERIAL_ECHOPGM("]\n"); - SERIAL_CHAR('\n'); + SERIAL_EOL; } #endif - return; - } + } #endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index cd211c96ba..b1f8946e50 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -154,7 +154,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(end[X_AXIS]) - pgm_read_float(&(ubl.mesh_index_to_xpos[cell_dest_xi]))) * (1.0 / (MESH_X_DIST)), + 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 * @@ -163,7 +163,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(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]) - pgm_read_float(&ubl.mesh_index_to_ypos[cell_dest_yi])) * (1.0 / (MESH_Y_DIST)); float z0 = z1 + (z2 - z1) * yratio; @@ -198,8 +198,8 @@ 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 : 0.0, - down_flag = dy < 0.0 ? 1.0 : 0.0; + const int left_flag = dx < 0.0 ? 1 : 0, + down_flag = dy < 0.0 ? 1 : 0; const float adx = left_flag ? -dx : dx, ady = down_flag ? -dy : dy; @@ -230,8 +230,8 @@ const float m = dy / dx, c = start[Y_AXIS] - m * start[X_AXIS]; - const bool inf_normalized_flag=isinf(e_normalized_dist), - inf_m_flag=isinf(m); + const bool inf_normalized_flag = isinf(e_normalized_dist), + inf_m_flag = isinf(m); /** * This block handles vertical lines. These are lines that stay within the same * X Cell column. They do not need to be perfectly vertical. They just can @@ -241,7 +241,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(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi])); /** * if the slope of the line is infinite, we won't do the calculations @@ -263,7 +263,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(pgm_read_float(&ubl.mesh_index_to_ypos[current_yi])); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -274,7 +274,7 @@ if (y != start[Y_AXIS]) { if (!inf_normalized_flag) { - //on_axis_distance = y - start[Y_AXIS]; + //on_axis_distance = y - start[Y_AXIS]; on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; //on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS]; @@ -283,7 +283,7 @@ //on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS]; //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; + e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } else { @@ -321,7 +321,7 @@ // 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(pgm_read_float(&ubl.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); @@ -337,7 +337,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(pgm_read_float(&ubl.mesh_index_to_xpos[current_xi])); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -393,8 +393,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(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])), 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. diff --git a/Marlin/vector_3.cpp b/Marlin/vector_3.cpp index 731aff813a..3471fedcac 100644 --- a/Marlin/vector_3.cpp +++ b/Marlin/vector_3.cpp @@ -63,7 +63,7 @@ vector_3 vector_3::get_normal() { return normalized; } -float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); } +float vector_3::get_length() { return sqrt(sq(x) + sq(y) + sq(z)); } void vector_3::normalize() { const float inv_length = 1.0 / get_length(); @@ -81,8 +81,8 @@ void vector_3::apply_rotation(matrix_3x3 matrix) { z = resultZ; } -void vector_3::debug(const char title[]) { - SERIAL_PROTOCOL(title); +void vector_3::debug(const char * const title) { + serialprintPGM(title); SERIAL_PROTOCOLPGM(" x: "); SERIAL_PROTOCOL_F(x, 6); SERIAL_PROTOCOLPGM(" y: "); @@ -101,14 +101,14 @@ void apply_rotation_xyz(matrix_3x3 matrix, float &x, float &y, float &z) { } matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) { - //row_0.debug("row_0"); - //row_1.debug("row_1"); - //row_2.debug("row_2"); + //row_0.debug(PSTR("row_0")); + //row_1.debug(PSTR("row_1")); + //row_2.debug(PSTR("row_2")); matrix_3x3 new_matrix; new_matrix.matrix[0] = row_0.x; new_matrix.matrix[1] = row_0.y; new_matrix.matrix[2] = row_0.z; new_matrix.matrix[3] = row_1.x; new_matrix.matrix[4] = row_1.y; new_matrix.matrix[5] = row_1.z; new_matrix.matrix[6] = row_2.x; new_matrix.matrix[7] = row_2.y; new_matrix.matrix[8] = row_2.z; - //new_matrix.debug("new_matrix"); + //new_matrix.debug(PSTR("new_matrix")); return new_matrix; } @@ -123,14 +123,14 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) { vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal(); vector_3 y_row = vector_3::cross(z_row, x_row).get_normal(); - // x_row.debug("x_row"); - // y_row.debug("y_row"); - // z_row.debug("z_row"); + // x_row.debug(PSTR("x_row")); + // y_row.debug(PSTR("y_row")); + // z_row.debug(PSTR("z_row")); // create the matrix already correctly transposed matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); - // rot.debug("rot"); + // rot.debug(PSTR("rot")); return rot; } @@ -142,8 +142,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) { return new_matrix; } -void matrix_3x3::debug(const char title[]) { - SERIAL_PROTOCOLLN(title); +void matrix_3x3::debug(const char * const title) { + serialprintPGM(title); uint8_t count = 0; for (uint8_t i = 0; i < 3; i++) { for (uint8_t j = 0; j < 3; j++) { diff --git a/Marlin/vector_3.h b/Marlin/vector_3.h index efc0132fe5..23ef745a1a 100644 --- a/Marlin/vector_3.h +++ b/Marlin/vector_3.h @@ -42,6 +42,7 @@ #define VECTOR_3_H #if HAS_ABL + class matrix_3x3; struct vector_3 { @@ -58,7 +59,7 @@ struct vector_3 { float get_length(); vector_3 get_normal(); - void debug(const char title[]); + void debug(const char * const title); void apply_rotation(matrix_3x3 matrix); }; @@ -72,11 +73,11 @@ struct matrix_3x3 { void set_to_identity(); - void debug(const char title[]); + void debug(const char * const title); }; void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z); -#endif // HAS_ABL +#endif // HAS_ABL #endif // VECTOR_3_H