SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Merge pull request #6552 from thinkyhead/rc_more_ubl_cleanup

Browse source 
Further cleanup of UBL
This commit is contained in:
Scott Lahteine 2017-05-02 22:46:04 -05:00 committed by GitHub
parent 902fe76db9 7ba7474a73
commit 628391304f
13 changed files with 456 additions and 483 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

20
Marlin/G26_Mesh_Validation_Tool.cpp
View file

@ -258,8 +258,8 @@
: find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. : 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) { if (location.x_index >= 0 && location.y_index >= 0) {
const float circle_x = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_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])); 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 // 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 #ifdef DELTA
@ -401,8 +401,8 @@
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (!is_bit_set(circle_flags, i, 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 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])); my = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
// Get the distance to this intersection // Get the distance to this intersection
float f = HYPOT(X - mx, Y - my); float f = HYPOT(X - mx, Y - my);
@ -446,11 +446,11 @@
// 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 = pgm_read_float(&(ubl.mesh_index_to_xpos[ i ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // right 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 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); 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); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (ubl.g26_debug_flag) { if (ubl.g26_debug_flag) {
@ -477,10 +477,10 @@
// 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!
// //
sy = pgm_read_float(&(ubl.mesh_index_to_ypos[ j ])) + (SIZE_OF_INTERSECTION_CIRCLES - (SIZE_OF_CROSSHAIRS)); // top 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 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); sy = constrain(sy, Y_MIN_POS + 1, Y_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);

14
Marlin/Marlin_main.cpp
View file

@ -4919,7 +4919,7 @@ void home_all_axes() { gcode_G28(); }
// For LINEAR and 3POINT leveling correct the current position // For LINEAR and 3POINT leveling correct the current position
if (verbose_level > 0) 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) { if (!dryrun) {
// //
@ -6965,7 +6965,7 @@ inline void gcode_M111() {
for (uint8_t i = 0; i < COUNT(debug_strings); i++) { for (uint8_t i = 0; i < COUNT(debug_strings); i++) {
if (TEST(marlin_debug_flags, i)) { if (TEST(marlin_debug_flags, i)) {
if (comma++) SERIAL_CHAR(','); 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 // V to print the matrix or mesh
if (code_seen('V')) { if (code_seen('V')) {
#if ABL_PLANAR #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) #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
if (bilinear_grid_spacing[X_AXIS]) { if (bilinear_grid_spacing[X_AXIS]) {
print_bilinear_leveling_grid(); 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 ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
tmp_offset_vec.debug("tmp_offset_vec"); tmp_offset_vec.debug(PSTR("tmp_offset_vec"));
act_offset_vec.debug("act_offset_vec"); act_offset_vec.debug(PSTR("act_offset_vec"));
offset_vec.debug("offset_vec (BEFORE)"); offset_vec.debug(PSTR("offset_vec (BEFORE)"));
} }
#endif #endif
offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix)); offset_vec.apply_rotation(planner.bed_level_matrix.transpose(planner.bed_level_matrix));
#if ENABLED(DEBUG_LEVELING_FEATURE) #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 #endif
// Adjustments to the current position // Adjustments to the current position

2
Marlin/example_configurations/delta/FLSUN/auto_calibrate/Configuration.h
View file

@ -452,7 +452,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
#define DELTA_AUTO_CALIBRATION #define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

2
Marlin/example_configurations/delta/FLSUN/kossel_mini/Configuration.h
View file

@ -459,7 +459,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 17) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

2
Marlin/example_configurations/delta/generic/Configuration.h
View file

@ -448,7 +448,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

2
Marlin/example_configurations/delta/kossel_mini/Configuration.h
View file

@ -448,7 +448,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 18) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

2
Marlin/example_configurations/delta/kossel_pro/Configuration.h
View file

@ -435,7 +435,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 25.4) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

2
Marlin/example_configurations/delta/kossel_xl/Configuration.h
View file

@ -453,7 +453,7 @@
// set the radius for the calibration probe points - max 0.8 * DELTA_PRINTABLE_RADIUS if DELTA_AUTO_CALIBRATION enabled // 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 #define DELTA_CALIBRATION_RADIUS (DELTA_PRINTABLE_RADIUS - 28) // mm
// G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results) // G33 Delta Auto-Calibration (Enable EEPROM_SETTINGS to store results)
//#define DELTA_AUTO_CALIBRATION //#define DELTA_AUTO_CALIBRATION
#if ENABLED(DELTA_AUTO_CALIBRATION) #if ENABLED(DELTA_AUTO_CALIBRATION)

12
Marlin/least_squares_fit.cpp
View file

@ -66,12 +66,12 @@ int finish_incremental_LSF(struct linear_fit_data *lsf) {
lsf->xbar /= N; lsf->xbar /= N;
lsf->ybar /= N; lsf->ybar /= N;
lsf->zbar /= N; lsf->zbar /= N;
lsf->x2bar = lsf->x2bar / N - lsf->xbar * lsf->xbar; lsf->x2bar = lsf->x2bar / N - sq(lsf->xbar);
lsf->y2bar = lsf->y2bar / N - lsf->ybar * lsf->ybar; lsf->y2bar = lsf->y2bar / N - sq(lsf->ybar);
lsf->z2bar = lsf->z2bar / N - lsf->zbar * lsf->zbar; lsf->z2bar = lsf->z2bar / N - sq(lsf->zbar);
lsf->xybar = lsf->xybar / N - lsf->xbar * lsf->ybar; lsf->xybar = lsf->xybar / N - sq(lsf->xbar);
lsf->yzbar = lsf->yzbar / N - lsf->ybar * lsf->zbar; lsf->yzbar = lsf->yzbar / N - sq(lsf->ybar);
lsf->xzbar = lsf->xzbar / N - lsf->xbar * lsf->zbar; lsf->xzbar = lsf->xzbar / N - sq(lsf->xbar);
const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar); const float DD = lsf->x2bar * lsf->y2bar - sq(lsf->xybar);
if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy)) if (fabs(DD) <= 1e-10 * (lsf->max_absx + lsf->max_absy))

820
Marlin/ubl_G29.cpp
View file

@ -31,10 +31,14 @@
#include "hex_print_routines.h" #include "hex_print_routines.h"
#include "configuration_store.h" #include "configuration_store.h"
#include "ultralcd.h" #include "ultralcd.h"
#include "stepper.h"
#include <math.h> #include <math.h>
#include "least_squares_fit.h" #include "least_squares_fit.h"
extern float destination[XYZE];
extern float current_position[XYZE];
void lcd_return_to_status(); void lcd_return_to_status();
bool lcd_clicked(); bool lcd_clicked();
void lcd_implementation_clear(); void lcd_implementation_clear();
@ -317,6 +321,7 @@
void __attribute__((optimize("O0"))) gcode_G29() { void __attribute__((optimize("O0"))) gcode_G29() {
if (ubl.eeprom_start < 0) { if (ubl.eeprom_start < 0) {
SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it"); SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it");
SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n"); SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n");
@ -347,7 +352,6 @@
} }
if (code_seen('Q')) { if (code_seen('Q')) {
const int test_pattern = code_has_value() ? code_value_int() : -1; const int test_pattern = code_has_value() ? code_value_int() : -1;
if (!WITHIN(test_pattern, 0, 2)) { if (!WITHIN(test_pattern, 0, 2)) {
SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n"); SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
@ -428,15 +432,16 @@
// //
SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n"); SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n");
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
if (!x_flag && !y_flag) { // use a good default location for the path if (!x_flag && !y_flag) {
// The flipped > and < operators on these two comparisons is /**
// intentional. It should cause the probed points to follow a * Use a good default location for the path.
// nice path on Cartesian printers. It may make sense to * The flipped > and < operators in these comparisons is intentional.
// have Delta printers default to the center of the bed. * It should cause the probed points to follow a nice path on Cartesian printers.
// For now, until that is decided, it can be forced with the X * It may make sense to have Delta printers default to the center of the bed.
// and Y parameters. * Until that is decided, this 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; 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')) { 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')); manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M'));
SERIAL_PROTOCOLLNPGM("G29 P2 finished");
} break; } break;
case 3: { case 3: {
// /**
// Populate invalid Mesh areas. Two choices are available to the user. The user can * Populate invalid mesh areas. Proceed with caution.
// specify the constant to be used with a C # paramter. Or the user can allow the G29 P3 command to * Two choices are available:
// apply a 'reasonable' constant to the invalid mesh point. Some caution and scrutiny should be used * - Specify a constant with the 'C' parameter.
// on either of these paths! * - Allow 'G29 P3' to choose a 'reasonable' constant.
// */
if (c_flag) { 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); 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 if (location.x_index < 0) break; // No more invalid Mesh Points to populate
ubl.z_values[location.x_index][location.y_index] = ubl_constant; ubl.z_values[location.x_index][location.y_index] = ubl_constant;
} }
break; 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: case 4:
// //
@ -483,55 +490,19 @@
// //
fine_tune_mesh(x_pos, y_pos, code_seen('O') || code_seen('M')); fine_tune_mesh(x_pos, y_pos, code_seen('O') || code_seen('M'));
break; break;
case 5:
ubl.find_mean_mesh_height();
break;
case 6:
ubl.shift_mesh_height();
break;
case 10: case 5: ubl.find_mean_mesh_height(); break;
// [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 11: case 6: ubl.shift_mesh_height(); break;
// [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;
} }
} }
if (code_seen('T')) { 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), 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), 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); 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 // 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)
@ -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 */ ; 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 */ ; 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.tilt_mesh_based_on_3pts(z1, z2, z3);
ubl.restore_ubl_active_state_and_leave(); ubl.restore_ubl_active_state_and_leave();
} }
@ -600,8 +571,8 @@
SERIAL_ECHOPAIR(" J ", y); SERIAL_ECHOPAIR(" J ", y);
SERIAL_ECHOPGM(" Z "); SERIAL_ECHOPGM(" Z ");
SERIAL_ECHO_F(ubl.z_values[x][y], 6); 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(" ; 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(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[y])));
SERIAL_EOL; SERIAL_EOL;
} }
return; return;
@ -647,9 +618,9 @@
} while (!ubl_lcd_clicked()); } while (!ubl_lcd_clicked());
ubl.has_control_of_lcd_panel = true; // There is a race condition for the Encoder Wheel getting 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) // 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, // or here. So, until we are done looking for a long Encoder Wheel Press,
// we need to take control of the panel // we need to take control of the panel
KEEPALIVE_STATE(IN_HANDLER); KEEPALIVE_STATE(IN_HANDLER);
@ -686,44 +657,39 @@
} }
void unified_bed_leveling::find_mean_mesh_height() { void unified_bed_leveling::find_mean_mesh_height() {
uint8_t x, y; float sum = 0.0;
int n; int n = 0;
float sum, sum_of_diff_squared, sigma, difference, mean; for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
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++)
if (!isnan(ubl.z_values[x][y])) { if (!isnan(ubl.z_values[x][y])) {
sum += ubl.z_values[x][y]; sum += ubl.z_values[x][y];
n++; n++;
} }
mean = sum / n; const float mean = sum / n;
// //
// Now do the sumation of the squares of difference from mean // Now do the sumation of the squares of difference from mean
// //
for (x = 0; x < GRID_MAX_POINTS_X; x++) float sum_of_diff_squared = 0.0;
for (y = 0; y < GRID_MAX_POINTS_Y; y++) for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
if (!isnan(ubl.z_values[x][y])) { for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
difference = (ubl.z_values[x][y] - mean); if (!isnan(ubl.z_values[x][y]))
sum_of_diff_squared += difference * difference; sum_of_diff_squared += sq(ubl.z_values[x][y] - mean);
}
SERIAL_ECHOLNPAIR("# of samples: ", n); SERIAL_ECHOLNPAIR("# of samples: ", n);
SERIAL_ECHOPGM("Mean Mesh Height: "); SERIAL_ECHOPGM("Mean Mesh Height: ");
SERIAL_ECHO_F(mean, 6); SERIAL_ECHO_F(mean, 6);
SERIAL_EOL; 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_ECHOPGM("Standard Deviation: ");
SERIAL_ECHO_F(sigma, 6); SERIAL_ECHO_F(sigma, 6);
SERIAL_EOL; SERIAL_EOL;
if (c_flag) if (c_flag)
for (x = 0; x < GRID_MAX_POINTS_X; x++) for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (y = 0; y < GRID_MAX_POINTS_Y; y++) for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
if (!isnan(ubl.z_values[x][y])) if (!isnan(ubl.z_values[x][y]))
ubl.z_values[x][y] -= mean + ubl_constant; 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); 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) { if (location.x_index >= 0 && location.y_index >= 0) {
const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_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])); rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility) // 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)) { 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(); ubl.restore_ubl_active_state_and_leave();
do_blocking_move_to_xy( do_blocking_move_to_xy(
constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS), constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X),
constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS) 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) { 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; int i, j;
matrix_3x3 rotation; matrix_3x3 rotation;
@ -812,94 +777,96 @@
* However, we don't know its direction. We need it to point up. So if * 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 * 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.x = -normal.x;
normal.y = -normal.y; normal.y = -normal.y;
normal.z = -normal.z; 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_ECHOPGM("bed plane normal = [");
SERIAL_PROTOCOL_F( normal.x, 7); SERIAL_PROTOCOL_F(normal.x, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.y, 7); SERIAL_PROTOCOL_F(normal.y, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.z, 7); SERIAL_PROTOCOL_F(normal.z, 7);
SERIAL_ECHOPGM("]\n"); SERIAL_ECHOLNPGM("]");
rotation.debug("rotation matrix:"); rotation.debug(PSTR("rotation matrix:"));
} }
// //
// 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_PROBE_PT_1_X + normal.y * UBL_PROBE_PT_1_Y; float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
d = t + normal.z * z1; d = t + normal.z * z1;
if (g29_verbose_level>2) { if (g29_verbose_level>2) {
SERIAL_ECHOPGM("D constant: "); SERIAL_ECHOPGM("D constant: ");
SERIAL_PROTOCOL_F( d, 7); SERIAL_PROTOCOL_F(d, 7);
SERIAL_ECHOPGM(" \n"); SERIAL_ECHOLNPGM(" ");
} }
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM("d from 1st point: "); SERIAL_ECHOPGM("d from 1st point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.x * UBL_PROBE_PT_2_X + normal.y * UBL_PROBE_PT_2_Y; t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
d = t + normal.z * z2; d = t + normal.z * z2;
SERIAL_ECHOPGM("d from 2nd point: "); SERIAL_ECHOPGM("d from 2nd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.x * UBL_PROBE_PT_3_X + normal.y * UBL_PROBE_PT_3_Y; t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
d = t + normal.z * z3; d = t + normal.z * z3;
SERIAL_ECHOPGM("d from 3rd point: "); SERIAL_ECHOPGM("d from 3rd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
SERIAL_EOL; SERIAL_EOL;
} }
#endif #endif
for (i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (j = 0; j < GRID_MAX_POINTS_Y; j++) { for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
float x_tmp, y_tmp, z_tmp; float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]),
x_tmp = pgm_read_float(ubl.mesh_index_to_xpos[i]); y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]),
y_tmp = pgm_read_float(ubl.mesh_index_to_ypos[j]); z_tmp = ubl.z_values[i][j];
z_tmp = ubl.z_values[i][j]; #if ENABLED(DEBUG_LEVELING_FEATURE)
#if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) {
if (DEBUGGING(LEVELING)) { SERIAL_ECHOPGM("before rotation = [");
SERIAL_ECHOPGM("before rotation = ["); SERIAL_PROTOCOL_F(x_tmp, 7);
SERIAL_PROTOCOL_F( x_tmp, 7); SERIAL_PROTOCOLCHAR(',');
SERIAL_ECHOPGM(","); SERIAL_PROTOCOL_F(y_tmp, 7);
SERIAL_PROTOCOL_F( y_tmp, 7); SERIAL_PROTOCOLCHAR(',');
SERIAL_ECHOPGM(","); SERIAL_PROTOCOL_F(z_tmp, 7);
SERIAL_PROTOCOL_F( z_tmp, 7); SERIAL_ECHOPGM("] ---> ");
SERIAL_ECHOPGM("] ---> "); safe_delay(20);
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() { 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); KEEPALIVE_STATE(PAUSED_FOR_USER);
while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here! while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here!
idle(); idle();
@ -912,21 +879,29 @@
return current_position[Z_AXIS]; 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) { float measure_business_card_thickness(const float &in_height) {
ubl.has_control_of_lcd_panel = true; 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_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); 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); //, 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(); const float z1 = use_encoder_wheel_to_measure_point();
do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); 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(); const float z2 = use_encoder_wheel_to_measure_point();
do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); 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_PROTOCOL_F(abs(z1 - z2), 6);
SERIAL_PROTOCOLLNPGM("mm thick."); SERIAL_PROTOCOLLNPGM("mm thick.");
} }
ubl.has_control_of_lcd_panel = false;
ubl.restore_ubl_active_state_and_leave(); ubl.restore_ubl_active_state_and_leave();
return abs(z1 - z2); 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. // 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; if (location.x_index < 0 && location.y_index < 0) continue;
const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_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])); rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility) // 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_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to probe off the bed."); SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
ubl.has_control_of_lcd_panel = false; 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 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(); idle();
if (ubl.encoder_diff) { if (ubl.encoder_diff) {
do_blocking_move_to_z(current_position[Z_AXIS] + float(ubl.encoder_diff) / 100.0); 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); 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 g29_parameter_parsing() {
bool err_flag = false; bool err_flag = false;
LCD_MESSAGEPGM("Doing G29 UBL!"); LCD_MESSAGEPGM("Doing G29 UBL!");
lcd_quick_feedback();
ubl_constant = 0.0; ubl_constant = 0.0;
repetition_cnt = 0; repetition_cnt = 0;
lcd_quick_feedback();
x_flag = code_seen('X') && code_has_value(); x_flag = code_seen('X') && code_has_value();
x_pos = x_flag ? code_value_float() : current_position[X_AXIS]; x_pos = x_flag ? code_value_float() : current_position[X_AXIS];
y_flag = code_seen('Y') && code_has_value(); y_flag = code_seen('Y') && code_has_value();
y_pos = y_flag ? code_value_float() : current_position[Y_AXIS]; y_pos = y_flag ? code_value_float() : current_position[Y_AXIS];
@ -1042,14 +1032,14 @@
if (repeat_flag) { if (repeat_flag) {
repetition_cnt = code_has_value() ? code_value_int() : (GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y); repetition_cnt = code_has_value() ? code_value_int() : (GRID_MAX_POINTS_X) * (GRID_MAX_POINTS_Y);
if (repetition_cnt < 1) { if (repetition_cnt < 1) {
SERIAL_PROTOCOLLNPGM("Invalid Repetition count.\n"); SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n");
return UBL_ERR; return UBL_ERR;
} }
} }
g29_verbose_level = code_seen('V') ? code_value_int() : 0; g29_verbose_level = code_seen('V') ? code_value_int() : 0;
if (!WITHIN(g29_verbose_level, 0, 4)) { 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; err_flag = true;
} }
@ -1066,44 +1056,47 @@
err_flag = true; 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"); SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n");
err_flag = true; 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"); SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n");
err_flag = true; err_flag = true;
} }
if (err_flag) return UBL_ERR; 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; ubl.state.active = 1;
SERIAL_PROTOCOLLNPGM("Unified Bed Leveling System activated.\n"); report_ubl_state();
} }
else if (code_seen('D')) {
c_flag = code_seen('C');
if (c_flag)
ubl_constant = code_value_float();
if (code_seen('D')) { // Disable the Unified Bed Leveling System
ubl.state.active = 0; 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 ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (code_seen('F') && code_has_value()) { if (code_seen('F') && code_has_value()) {
const float fh = code_value_float(); const float fh = code_value_float();
if (!WITHIN(fh, 0.0, 100.0)) { 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; return UBL_ERR;
} }
set_z_fade_height(fh); set_z_fade_height(fh);
} }
#endif #endif
map_type = code_seen('O') && code_has_value() ? code_value_int() : 0; map_type = code_seen('O') && code_has_value() ? code_value_int() : 0;
if (!WITHIN(map_type, 0, 1)) { if (!WITHIN(map_type, 0, 1)) {
SERIAL_PROTOCOLLNPGM("Invalid map type.\n"); 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 * This function goes away after G29 debug is complete. But for right now, it is a handy
* routine to dump binary data structures. * routine to dump binary data structures.
*/ */
/* /*
void dump(char * const str, const float &f) { void dump(char * const str, const float &f) {
char *ptr; char *ptr;
@ -1143,7 +1136,7 @@
SERIAL_EOL; SERIAL_EOL;
} }
*/ //*/
static int ubl_state_at_invocation = 0, static int ubl_state_at_invocation = 0,
ubl_state_recursion_chk = 0; ubl_state_recursion_chk = 0;
@ -1170,7 +1163,6 @@
ubl.state.active = ubl_state_at_invocation; ubl.state.active = ubl_state_at_invocation;
} }
/** /**
* Much of the 'What?' command can be eliminated. But until we are fully debugged, it is * 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 * good to have the extra information. Soon... we prune this to just a few items
@ -1178,7 +1170,8 @@
void g29_what_command() { void g29_what_command() {
const uint16_t k = E2END - ubl.eeprom_start; 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) if (ubl.state.active)
SERIAL_PROTOCOLCHAR('A'); SERIAL_PROTOCOLCHAR('A');
else else
@ -1209,11 +1202,11 @@
SERIAL_EOL; SERIAL_EOL;
safe_delay(25); 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: "); SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: ");
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { 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(" "); SERIAL_PROTOCOLPGM(" ");
safe_delay(50); safe_delay(50);
} }
@ -1221,7 +1214,7 @@
SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) { 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(" "); SERIAL_PROTOCOLPGM(" ");
safe_delay(50); safe_delay(50);
} }
@ -1275,8 +1268,10 @@
SERIAL_EOL; SERIAL_EOL;
safe_delay(50); safe_delay(50);
if (!ubl.sanity_check()) if (!ubl.sanity_check()) {
SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed."); say_ubl_name();
SERIAL_PROTOCOLLNPGM("sanity checks passed.");
}
} }
/** /**
@ -1336,18 +1331,18 @@
ubl.z_values[x][y] -= tmp_z_values[x][y]; 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) { 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) {
float distance, closest = far_flag ? -99999.99 : 99999.99; mesh_index_pair out_mesh;
mesh_index_pair return_val; out_mesh.x_index = out_mesh.y_index = -1;
return_val.x_index = return_val.y_index = -1;
const float current_x = current_position[X_AXIS], const float current_x = current_position[X_AXIS],
current_y = current_position[Y_AXIS]; current_y = current_position[Y_AXIS];
// Get our reference position. Either the nozzle or probe location. // 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), 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); 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 i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) { 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 // 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 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])); rawy = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
// If using the probe as the reference there are some unreachable locations. // 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). // 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)) (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y))
) continue; ) continue;
@ -1375,30 +1370,38 @@
const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location
my = LOGICAL_Y_POSITION(rawy); 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 * If doing the far_flag action, we want to be as far as possible
for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) { // want the next point spread out and filling in any blank spaces * from the starting point and from any other probed points. We
if (!isnan(ubl.z_values[k][l])) { // in the mesh. So we add in some of the distance to every probed * want the next point spread out and filling in any blank spaces
distance += sq(i - k) * (MESH_X_DIST) * .05 // point we can find. * 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; + 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 closest = distance; // We found a closer/farther location with
return_val.x_index = i; // the specified type of mesh value. out_mesh.x_index = i; // the specified type of mesh value.
return_val.y_index = j; out_mesh.y_index = j;
return_val.distance = closest; out_mesh.distance = closest;
} }
} }
} // for j } // for j
} // for i } // for i
return return_val; return out_mesh;
} }
void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) { 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_blocking_move_to_xy(lx, ly);
do { do {
location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false); 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 // It doesn't matter if the probe can't reach this
// location. This is a manual edit of the Mesh Point. // 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. 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 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 // different location the next time through the loop
const float rawx = pgm_read_float(&(ubl.mesh_index_to_xpos[location.x_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])); rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
// TODO: Change to use `position_is_reachable` (for SCARA-compatibility) // 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_ERROR_START;
SERIAL_ERRORLNPGM("Attempt to edit off the bed."); // This really can't happen, but do the check for now 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; ubl.has_control_of_lcd_panel = false;
goto FINE_TUNE_EXIT; 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]; 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 if (!isnan(new_z)) { //can't fine tune a point that hasn't been probed
new_z = float(round_off) / 1000.0;
KEEPALIVE_STATE(PAUSED_FOR_USER); do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit
ubl.has_control_of_lcd_panel = true; 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(); KEEPALIVE_STATE(PAUSED_FOR_USER);
lcd_mesh_edit_setup(new_z); ubl.has_control_of_lcd_panel = true;
do { if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted
new_z = lcd_mesh_edit();
idle();
} while (!ubl_lcd_clicked());
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. lcd_mesh_edit_setup(new_z);
// It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
// or here. 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; const millis_t nxt = millis() + 1500UL;
while (ubl_lcd_clicked()) { // debounce and watch for abort while (ubl_lcd_clicked()) { // debounce and watch for abort
@ -1501,229 +1509,193 @@
SERIAL_ECHOLNPGM("Done Editing Mesh"); SERIAL_ECHOLNPGM("Done Editing Mesh");
} }
// /**
// The routine provides the 'Smart Fill' capability. It scans from the * 'Smart Fill': Scan from the outward edges of the mesh towards the center.
// outward edges of the mesh towards the center. If it finds an invalid * If an invalid location is found, use the next two points (if valid) to
// location, it uses the next two points (assumming they are valid) to * calculate a 'reasonable' value for the unprobed mesh point.
// calculate a 'reasonable' value for the unprobed mesh point. */
//
void smart_fill_mesh() { bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
float f, diff; const int8_t x1 = x + xdir, x2 = x1 + xdir,
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Bottom of the mesh looking up y1 = y + ydir, y2 = y1 + ydir;
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y-2; y++) { // A NAN next to a pair of real values?
if (isnan(ubl.z_values[x][y])) { if (isnan(ubl.z_values[x][y]) && !isnan(ubl.z_values[x1][y1]) && !isnan(ubl.z_values[x2][y2])) {
if (isnan(ubl.z_values[x][y+1])) // we only deal with the first NAN next to a block of if (ubl.z_values[x1][y1] < ubl.z_values[x2][y2]) // Angled downward?
continue; // good numbers. we want 2 good numbers to extrapolate off of. ubl.z_values[x][y] = ubl.z_values[x1][y1]; // Use nearest (maybe a little too high.)
if (isnan(ubl.z_values[x][y+2])) else {
continue; const float diff = ubl.z_values[x1][y1] - ubl.z_values[x2][y2]; // Angled upward
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[x1][y1] + diff; // Use closest plus difference
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 true;
} }
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Top of the mesh looking down return false;
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 typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info;
continue; // good numbers. we want 2 good numbers to extrapolate off of.
if (isnan(ubl.z_values[x][y-2])) void smart_fill_loop(const smart_fill_info &f) {
continue; if (f.yfirst) {
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 const int8_t dir = f.ex > f.sx ? 1 : -1;
ubl.z_values[x][y] = ubl.z_values[x][y-1]; // use the closest value, which is probably a little too high for (uint8_t y = f.sy; y != f.ey; ++y)
else { for (uint8_t x = f.sx; x != f.ex; x += dir)
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 if (smart_fill_one(x, y, dir, 0)) break;
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;
}
}
} }
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { else {
for (uint8_t x = 0; x < GRID_MAX_POINTS_X-2; x++) { // Left side of the mesh looking right const int8_t dir = f.ey > f.sy ? 1 : -1;
if (isnan(ubl.z_values[x][y])) { for (uint8_t x = f.sx; x != f.ex; ++x)
if (isnan(ubl.z_values[x+1][y])) // we only deal with the first NAN next to a block of for (uint8_t y = f.sy; y != f.ey; y += dir)
continue; // good numbers. we want 2 good numbers to extrapolate off of. if (smart_fill_one(x, y, 0, dir)) break;
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;
}
}
} }
} }
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) { 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 constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
int8_t ix, iy, zig_zag=0, status; 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; 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); incremental_LSF_reset(&lsf_results);
for(ix=0; ix<grid_size; ix++) {
x = ((float)x_min) + ix*dx; bool zig_zag = false;
for(iy=0; iy<grid_size; iy++) { for (uint8_t ix = 0; ix < grid_size; ix++) {
if (zig_zag) const float x = float(x_min) + ix * dx;
y = ((float)y_min) + (grid_size-iy-1)*dy; for (int8_t iy = 0; iy < grid_size; iy++) {
else const float y = float(y_min) + dy * (zig_zag ? grid_size - 1 - iy : iy);
y = ((float)y_min) + iy*dy; float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level);
measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level); #if ENABLED(DEBUG_LEVELING_FEATURE)
#if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) {
if (DEBUGGING(LEVELING)) { SERIAL_CHAR('(');
SERIAL_ECHOPGM("("); SERIAL_PROTOCOL_F(x, 7);
SERIAL_PROTOCOL_F( x, 7); SERIAL_CHAR(',');
SERIAL_ECHOPGM(","); SERIAL_PROTOCOL_F(y, 7);
SERIAL_PROTOCOL_F( y, 7); SERIAL_ECHOPGM(") logical: ");
SERIAL_ECHOPGM(") logical: "); SERIAL_CHAR('(');
SERIAL_ECHOPGM("("); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
SERIAL_PROTOCOL_F( LOGICAL_X_POSITION(x), 7); SERIAL_CHAR(',');
SERIAL_ECHOPGM(","); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
SERIAL_PROTOCOL_F( LOGICAL_X_POSITION(y), 7); SERIAL_ECHOPGM(") measured: ");
SERIAL_ECHOPGM(") measured: "); SERIAL_PROTOCOL_F(measured_z, 7);
SERIAL_PROTOCOL_F( measured_z, 7); SERIAL_ECHOPGM(" correction: ");
SERIAL_ECHOPGM(" correction: "); SERIAL_PROTOCOL_F(ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
SERIAL_PROTOCOL_F( ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
} }
#endif #endif
measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
#if ENABLED(DEBUG_LEVELING_FEATURE) measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM(" final >>>---> "); #if ENABLED(DEBUG_LEVELING_FEATURE)
SERIAL_PROTOCOL_F( measured_z, 7); if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM("\n"); SERIAL_ECHOPGM(" final >>>---> ");
} SERIAL_PROTOCOL_F(measured_z, 7);
#endif SERIAL_EOL;
incremental_LSF(&lsf_results, x, y, measured_z); }
#endif
incremental_LSF(&lsf_results, x, y, measured_z);
} }
zig_zag = !zig_zag; zig_zag ^= true;
} }
status = finish_incremental_LSF(&lsf_results); const int status = finish_incremental_LSF(&lsf_results);
if (g29_verbose_level>3) {
if (g29_verbose_level > 3) {
SERIAL_ECHOPGM("LSF Results A="); SERIAL_ECHOPGM("LSF Results A=");
SERIAL_PROTOCOL_F( lsf_results.A, 7); SERIAL_PROTOCOL_F(lsf_results.A, 7);
SERIAL_ECHOPGM(" B="); SERIAL_ECHOPGM(" B=");
SERIAL_PROTOCOL_F( lsf_results.B, 7); SERIAL_PROTOCOL_F(lsf_results.B, 7);
SERIAL_ECHOPGM(" D="); SERIAL_ECHOPGM(" D=");
SERIAL_PROTOCOL_F( lsf_results.D, 7); SERIAL_PROTOCOL_F(lsf_results.D, 7);
SERIAL_CHAR('\n'); SERIAL_EOL;
} }
normal = vector_3( lsf_results.A, lsf_results.B, 1.0000); vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
normal = normal.get_normal();
if (g29_verbose_level>2) { if (g29_verbose_level > 2) {
SERIAL_ECHOPGM("bed plane normal = ["); SERIAL_ECHOPGM("bed plane normal = [");
SERIAL_PROTOCOL_F( normal.x, 7); SERIAL_PROTOCOL_F(normal.x, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.y, 7); SERIAL_PROTOCOL_F(normal.y, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.z, 7); SERIAL_PROTOCOL_F(normal.z, 7);
SERIAL_ECHOPGM("]\n"); 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 ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
rotation.debug("rotation matrix:"); rotation.debug(PSTR("rotation matrix:"));
SERIAL_ECHOPGM("LSF Results A="); SERIAL_ECHOPGM("LSF Results A=");
SERIAL_PROTOCOL_F( lsf_results.A, 7); SERIAL_PROTOCOL_F(lsf_results.A, 7);
SERIAL_ECHOPGM(" B="); SERIAL_ECHOPGM(" B=");
SERIAL_PROTOCOL_F( lsf_results.B, 7); SERIAL_PROTOCOL_F(lsf_results.B, 7);
SERIAL_ECHOPGM(" D="); SERIAL_ECHOPGM(" D=");
SERIAL_PROTOCOL_F( lsf_results.D, 7); SERIAL_PROTOCOL_F(lsf_results.D, 7);
SERIAL_CHAR('\n'); SERIAL_EOL;
safe_delay(55); safe_delay(55);
SERIAL_ECHOPGM("bed plane normal = ["); SERIAL_ECHOPGM("bed plane normal = [");
SERIAL_PROTOCOL_F( normal.x, 7); SERIAL_PROTOCOL_F(normal.x, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.y, 7); SERIAL_PROTOCOL_F(normal.y, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F( normal.z, 7); SERIAL_PROTOCOL_F(normal.z, 7);
SERIAL_ECHOPGM("]\n"); SERIAL_ECHOPGM("]\n");
SERIAL_CHAR('\n'); SERIAL_EOL;
} }
#endif #endif
return; }
}
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL

28
Marlin/ubl_motion.cpp
View file

@ -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. * 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 * 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 *
@ -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 // 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(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; float z0 = z1 + (z2 - z1) * yratio;
@ -198,8 +198,8 @@
const float dx = end[X_AXIS] - start[X_AXIS], const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS]; dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1.0 : 0.0, const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1.0 : 0.0; down_flag = dy < 0.0 ? 1 : 0;
const float adx = left_flag ? -dx : dx, const float adx = left_flag ? -dx : dx,
ady = down_flag ? -dy : dy; ady = down_flag ? -dy : dy;
@ -230,8 +230,8 @@
const float m = dy / dx, const float m = dy / dx,
c = start[Y_AXIS] - m * start[X_AXIS]; c = start[Y_AXIS] - m * start[X_AXIS];
const bool inf_normalized_flag=isinf(e_normalized_dist), const bool inf_normalized_flag = isinf(e_normalized_dist),
inf_m_flag=isinf(m); inf_m_flag = isinf(m);
/** /**
* This block handles vertical lines. These are lines that stay within the same * 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 * 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 current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) { while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi; 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 * if the slope of the line is infinite, we won't do the calculations
@ -263,7 +263,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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 * 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 (y != start[Y_AXIS]) {
if (!inf_normalized_flag) { 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 ? 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]; //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 ? 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]; //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; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
else { else {
@ -321,7 +321,7 @@
// edge of this cell for the first move. // edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) { while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi; 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 y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi); float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
@ -337,7 +337,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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 * 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) { 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]))), 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]))), 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 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 x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero. // (No need to worry about m being zero.

26
Marlin/vector_3.cpp
View file

@ -63,7 +63,7 @@ vector_3 vector_3::get_normal() {
return normalized; 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() { void vector_3::normalize() {
const float inv_length = 1.0 / get_length(); const float inv_length = 1.0 / get_length();
@ -81,8 +81,8 @@ void vector_3::apply_rotation(matrix_3x3 matrix) {
z = resultZ; z = resultZ;
} }
void vector_3::debug(const char title[]) { void vector_3::debug(const char * const title) {
SERIAL_PROTOCOL(title); serialprintPGM(title);
SERIAL_PROTOCOLPGM(" x: "); SERIAL_PROTOCOLPGM(" x: ");
SERIAL_PROTOCOL_F(x, 6); SERIAL_PROTOCOL_F(x, 6);
SERIAL_PROTOCOLPGM(" y: "); 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) { 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_0.debug(PSTR("row_0"));
//row_1.debug("row_1"); //row_1.debug(PSTR("row_1"));
//row_2.debug("row_2"); //row_2.debug(PSTR("row_2"));
matrix_3x3 new_matrix; 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[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[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.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; 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 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(); vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
// x_row.debug("x_row"); // x_row.debug(PSTR("x_row"));
// y_row.debug("y_row"); // y_row.debug(PSTR("y_row"));
// z_row.debug("z_row"); // z_row.debug(PSTR("z_row"));
// create the matrix already correctly transposed // create the matrix already correctly transposed
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row); matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
// rot.debug("rot"); // rot.debug(PSTR("rot"));
return rot; return rot;
} }
@ -142,8 +142,8 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
return new_matrix; return new_matrix;
} }
void matrix_3x3::debug(const char title[]) { void matrix_3x3::debug(const char * const title) {
SERIAL_PROTOCOLLN(title); serialprintPGM(title);
uint8_t count = 0; uint8_t count = 0;
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
for (uint8_t j = 0; j < 3; j++) { for (uint8_t j = 0; j < 3; j++) {

7
Marlin/vector_3.h
View file

@ -42,6 +42,7 @@
#define VECTOR_3_H #define VECTOR_3_H
#if HAS_ABL #if HAS_ABL
class matrix_3x3; class matrix_3x3;
struct vector_3 { struct vector_3 {
@ -58,7 +59,7 @@ struct vector_3 {
float get_length(); float get_length();
vector_3 get_normal(); vector_3 get_normal();
void debug(const char title[]); void debug(const char * const title);
void apply_rotation(matrix_3x3 matrix); void apply_rotation(matrix_3x3 matrix);
}; };
@ -72,11 +73,11 @@ struct matrix_3x3 {
void set_to_identity(); 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); void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
#endif // HAS_ABL
#endif // HAS_ABL
#endif // VECTOR_3_H #endif // VECTOR_3_H