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

Apply formatting, coding standards to UBL

Browse source
This commit is contained in:
Scott Lahteine 2017-05-02 17:41:55 -05:00
parent 492bc536bf
commit c4e34adcf1
3 changed files with 313 additions and 315 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);

378
Marlin/ubl_G29.cpp
View file

@ -352,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");
@ -433,13 +432,14 @@
// //
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 ? UBL_MESH_MAX_X : UBL_MESH_MIN_X; 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; y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? UBL_MESH_MAX_Y : UBL_MESH_MIN_Y;
} }
@ -461,16 +461,16 @@
} }
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"); 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);
@ -478,10 +478,11 @@
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:
// //
@ -606,8 +607,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;
@ -692,44 +693,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;
} }
@ -767,8 +763,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)) {
@ -797,7 +793,6 @@
} }
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;
@ -829,25 +824,25 @@
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)
@ -855,12 +850,12 @@
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);
@ -868,19 +863,18 @@
} }
#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_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(y_tmp, 7); SERIAL_PROTOCOL_F(y_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(z_tmp, 7); SERIAL_PROTOCOL_F(z_tmp, 7);
SERIAL_ECHOPGM("] ---> "); SERIAL_ECHOPGM("] ---> ");
safe_delay(20); safe_delay(20);
@ -891,23 +885,22 @@
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM("after rotation = ["); SERIAL_ECHOPGM("after rotation = [");
SERIAL_PROTOCOL_F(x_tmp, 7); SERIAL_PROTOCOL_F(x_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(y_tmp, 7); SERIAL_PROTOCOL_F(y_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(z_tmp, 7); SERIAL_PROTOCOL_F(z_tmp, 7);
SERIAL_ECHOPGM("]\n"); SERIAL_ECHOLNPGM("]");
safe_delay(55); safe_delay(55);
} }
#endif #endif
ubl.z_values[i][j] += z_tmp - d; 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 while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel
delay(50); // debounce delay(50); // debounce
KEEPALIVE_STATE(PAUSED_FOR_USER); KEEPALIVE_STATE(PAUSED_FOR_USER);
@ -922,24 +915,29 @@
return current_position[Z_AXIS]; return current_position[Z_AXIS];
} }
float measure_business_card_thickness(const float &in_height) { 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.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
do_blocking_move_to_z(in_height); do_blocking_move_to_z(in_height);
do_blocking_move_to_xy((float(UBL_MESH_MAX_X) - float(UBL_MESH_MIN_X)) / 2.0, (float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / 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(); stepper.synchronize();
SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement."); 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);
stepper.synchronize(); stepper.synchronize();
SERIAL_PROTOCOLLNPGM("Remove Shim and Measure Bed Height.");
SERIAL_PROTOCOLPGM("Remove shim");
say_and_take_a_measurement();
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);
@ -968,8 +966,8 @@
// 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, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) {
@ -999,10 +997,8 @@
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()) delay(50); // wait for user to release encoder wheel
while (ubl_lcd_clicked()) delay(50);; // wait for user to release encoder wheel
delay(50); // debounce delay(50); // debounce
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();
if (ubl.encoder_diff) { if (ubl.encoder_diff) {
@ -1011,7 +1007,6 @@
} }
} }
const millis_t nxt = millis() + 1500L; const millis_t nxt = millis() + 1500L;
while (ubl_lcd_clicked()) { // debounce and watch for abort while (ubl_lcd_clicked()) { // debounce and watch for abort
idle(); idle();
@ -1044,33 +1039,43 @@
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];
repeat_flag = code_seen('R'); repeat_flag = code_seen('R');
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;
} }
@ -1099,32 +1104,35 @@
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");
@ -1164,7 +1172,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;
@ -1191,7 +1199,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
@ -1199,7 +1206,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
@ -1230,11 +1238,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);
} }
@ -1242,7 +1250,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);
} }
@ -1296,8 +1304,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.");
}
} }
/** /**
@ -1357,11 +1367,9 @@
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];
@ -1370,6 +1378,8 @@
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++) {
@ -1380,8 +1390,8 @@
// 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).
@ -1396,30 +1406,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) {
@ -1439,15 +1457,15 @@
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, X_MIN_POS, X_MAX_POS) || !WITHIN(rawy, Y_MIN_POS, Y_MAX_POS)) { // In theory, we don't need this check.
@ -1464,45 +1482,31 @@
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit 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)); do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy));
round_off = (int32_t)(new_z * 1000.0); // we chop off the last digits just to be clean. We are rounding to the 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; new_z = float(round_off) / 1000.0;
KEEPALIVE_STATE(PAUSED_FOR_USER); KEEPALIVE_STATE(PAUSED_FOR_USER);
ubl.has_control_of_lcd_panel = true; ubl.has_control_of_lcd_panel = true;
if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted
lcd_implementation_clear(); lcd_implementation_clear();
lcd_mesh_edit_setup(new_z); lcd_mesh_edit_setup(new_z);
do { do {
new_z = lcd_mesh_edit(); new_z = lcd_mesh_edit();
idle(); idle();
} while (!ubl_lcd_clicked()); } while (!ubl_lcd_clicked());
lcd_return_to_status(); lcd_return_to_status();
// 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. // 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
idle(); idle();
@ -1621,41 +1625,33 @@
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_ECHOPGM("("); SERIAL_CHAR('(');
SERIAL_PROTOCOL_F(x, 7); SERIAL_PROTOCOL_F(x, 7);
SERIAL_ECHOPGM(","); SERIAL_CHAR(',');
SERIAL_PROTOCOL_F(y, 7); SERIAL_PROTOCOL_F(y, 7);
SERIAL_ECHOPGM(") logical: "); SERIAL_ECHOPGM(") logical: ");
SERIAL_ECHOPGM("("); SERIAL_CHAR('(');
SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
SERIAL_ECHOPGM(","); SERIAL_CHAR(',');
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);
@ -1663,22 +1659,25 @@
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 */ ; measured_z -= ubl.get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM(" final >>>---> "); SERIAL_ECHOPGM(" final >>>---> ");
SERIAL_PROTOCOL_F(measured_z, 7); SERIAL_PROTOCOL_F(measured_z, 7);
SERIAL_ECHOPGM("\n"); SERIAL_EOL;
} }
#endif #endif
incremental_LSF(&lsf_results, x, y, measured_z); 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);
@ -1686,55 +1685,55 @@
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("]");
} }
rotation = matrix_3x3::create_look_at( vector_3( lsf_results.A, lsf_results.B, 1)); matrix_3x3 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 (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_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(y_tmp, 7); SERIAL_PROTOCOL_F(y_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(z_tmp, 7); SERIAL_PROTOCOL_F(z_tmp, 7);
SERIAL_ECHOPGM("] ---> "); SERIAL_ECHOPGM("] ---> ");
safe_delay(20); safe_delay(20);
} }
#endif #endif
apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp); apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPGM("after rotation = ["); SERIAL_ECHOPGM("after rotation = [");
SERIAL_PROTOCOL_F(x_tmp, 7); SERIAL_PROTOCOL_F(x_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(y_tmp, 7); SERIAL_PROTOCOL_F(y_tmp, 7);
SERIAL_ECHOPGM(","); SERIAL_PROTOCOLCHAR(',');
SERIAL_PROTOCOL_F(z_tmp, 7); SERIAL_PROTOCOL_F(z_tmp, 7);
SERIAL_ECHOPGM("]\n"); SERIAL_ECHOLNPGM("]");
safe_delay(55); safe_delay(55);
} }
#endif #endif
ubl.z_values[i][j] += z_tmp - lsf_results.D; ubl.z_values[i][j] += z_tmp - lsf_results.D;
@ -1743,27 +1742,26 @@
#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

20
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;
@ -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
@ -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.