Merge pull request #6552 from thinkyhead/rc_more_ubl_cleanup
Further cleanup of UBL
This commit is contained in:
commit
628391304f
13 changed files with 456 additions and 483 deletions
|
@ -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);
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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))
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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.
|
||||||
|
|
|
@ -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++) {
|
||||||
|
|
|
@ -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
|
||||||
|
|
Reference in a new issue