andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Embed G26/G29 in ubl class, with enhancements

pull/1/head
Scott Lahteine 8 years ago
parent
c99bd69889
commit
85b967657e
7 changed files with 515 additions and 484 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 231
      Marlin/G26_Mesh_Validation_Tool.cpp
  2. 12
      Marlin/Marlin_main.cpp
  3. 2
      Marlin/fastio.h
  4. 8
      Marlin/ubl.cpp
  5. 161
      Marlin/ubl.h
  6. 453
      Marlin/ubl_G29.cpp
  7. 128
      Marlin/ubl_motion.cpp

231
Marlin/G26_Mesh_Validation_Tool.cpp
View File

@ -135,54 +135,44 @@
float code_value_axis_units(const AxisEnum axis); float code_value_axis_units(const AxisEnum axis);
bool code_value_bool(); bool code_value_bool();
bool code_has_value(); bool code_has_value();
void lcd_init();
void lcd_setstatuspgm(const char* const message, const uint8_t level);
void sync_plan_position_e(); void sync_plan_position_e();
void chirp_at_user(); void chirp_at_user();
// Private functions // Private functions
void un_retract_filament(float where[XYZE]);
void retract_filament(float where[XYZE]);
bool look_for_lines_to_connect();
bool parse_G26_parameters();
void move_to(const float&, const float&, const float&, const float&) ;
void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
bool turn_on_heaters();
bool prime_nozzle();
static uint16_t circle_flags[16], horizontal_mesh_line_flags[16], vertical_mesh_line_flags[16]; static uint16_t circle_flags[16], horizontal_mesh_line_flags[16], vertical_mesh_line_flags[16];
float g26_e_axis_feedrate = 0.020, float g26_e_axis_feedrate = 0.020,
random_deviation = 0.0, random_deviation = 0.0;
layer_height = LAYER_HEIGHT;
static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched
// retracts/recovers won't result in a bad state. // retracts/recovers won't result in a bad state.
float valid_trig_angle(float); float valid_trig_angle(float);
mesh_index_pair find_closest_circle_to_print(const float&, const float&);
static float extrusion_multiplier = EXTRUSION_MULTIPLIER, float unified_bed_leveling::g26_extrusion_multiplier,
retraction_multiplier = RETRACTION_MULTIPLIER, unified_bed_leveling::g26_retraction_multiplier,
nozzle = NOZZLE, unified_bed_leveling::g26_nozzle,
filament_diameter = FILAMENT, unified_bed_leveling::g26_filament_diameter,
prime_length = PRIME_LENGTH, unified_bed_leveling::g26_layer_height,
x_pos, y_pos, unified_bed_leveling::g26_prime_length,
ooze_amount = OOZE_AMOUNT; unified_bed_leveling::g26_x_pos,
unified_bed_leveling::g26_y_pos,
unified_bed_leveling::g26_ooze_amount;
static int16_t bed_temp = BED_TEMP, int16_t unified_bed_leveling::g26_bed_temp,
hotend_temp = HOTEND_TEMP; unified_bed_leveling::g26_hotend_temp;
static int8_t prime_flag = 0; int8_t unified_bed_leveling::g26_prime_flag;
static bool continue_with_closest, keep_heaters_on; bool unified_bed_leveling::g26_continue_with_closest,
unified_bed_leveling::g26_keep_heaters_on;
static int16_t g26_repeats; int16_t unified_bed_leveling::g26_repeats;
void G26_line_to_destination(const float &feed_rate) { void unified_bed_leveling::G26_line_to_destination(const float &feed_rate) {
const float save_feedrate = feedrate_mm_s; const float save_feedrate = feedrate_mm_s;
feedrate_mm_s = feed_rate; // use specified feed rate feedrate_mm_s = feed_rate; // use specified feed rate
prepare_move_to_destination(); // will ultimately call ubl_line_to_destination_cartesian or ubl_prepare_linear_move_to for UBL_DELTA prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_DELTA
feedrate_mm_s = save_feedrate; // restore global feed rate feedrate_mm_s = save_feedrate; // restore global feed rate
} }
@ -216,7 +206,7 @@
* Used to interactively edit UBL's Mesh by placing the * Used to interactively edit UBL's Mesh by placing the
* nozzle in a problem area and doing a G29 P4 R command. * nozzle in a problem area and doing a G29 P4 R command.
*/ */
void gcode_G26() { void unified_bed_leveling::G26() {
SERIAL_ECHOLNPGM("G26 command started. Waiting for heater(s)."); SERIAL_ECHOLNPGM("G26 command started. Waiting for heater(s).");
float tmp, start_angle, end_angle; float tmp, start_angle, end_angle;
int i, xi, yi; int i, xi, yi;
@ -237,7 +227,7 @@
current_position[E_AXIS] = 0.0; current_position[E_AXIS] = 0.0;
sync_plan_position_e(); sync_plan_position_e();
if (prime_flag && prime_nozzle()) goto LEAVE; if (g26_prime_flag && prime_nozzle()) goto LEAVE;
/** /**
* Bed is preheated * Bed is preheated
@ -255,11 +245,11 @@
// Move nozzle to the specified height for the first layer // Move nozzle to the specified height for the first layer
set_destination_to_current(); set_destination_to_current();
destination[Z_AXIS] = layer_height; destination[Z_AXIS] = g26_layer_height;
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0); move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0.0);
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], ooze_amount); move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], g26_ooze_amount);
ubl.has_control_of_lcd_panel = true; has_control_of_lcd_panel = true;
//debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern.")); //debug_current_and_destination(PSTR("Starting G26 Mesh Validation Pattern."));
/** /**
@ -273,13 +263,13 @@
} }
do { do {
location = continue_with_closest location = g26_continue_with_closest
? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS]) ? find_closest_circle_to_print(current_position[X_AXIS], current_position[Y_AXIS])
: find_closest_circle_to_print(x_pos, y_pos); // Find the closest Mesh Intersection to where we are now. : find_closest_circle_to_print(g26_x_pos, g26_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 = mesh_index_to_xpos(location.x_index),
circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); circle_y = mesh_index_to_ypos(location.y_index);
// If this mesh location is outside the printable_radius, skip it. // If this mesh location is outside the printable_radius, skip it.
@ -288,7 +278,7 @@
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index; yi = location.y_index;
if (ubl.g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" Doing circle at: (xi=", xi); SERIAL_ECHOPAIR(" Doing circle at: (xi=", xi);
SERIAL_ECHOPAIR(", yi=", yi); SERIAL_ECHOPAIR(", yi=", yi);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
@ -344,7 +334,7 @@
ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1); ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
#endif #endif
//if (ubl.g26_debug_flag) { //if (g26_debug_flag) {
// char ccc, *cptr, seg_msg[50], seg_num[10]; // char ccc, *cptr, seg_msg[50], seg_num[10];
// strcpy(seg_msg, " segment: "); // strcpy(seg_msg, " segment: ");
// strcpy(seg_num, " \n"); // strcpy(seg_num, " \n");
@ -355,7 +345,7 @@
// debug_current_and_destination(seg_msg); // debug_current_and_destination(seg_msg);
//} //}
print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), layer_height); print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), g26_layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), g26_layer_height);
} }
if (look_for_lines_to_connect()) if (look_for_lines_to_connect())
@ -374,16 +364,16 @@
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Raise the nozzle
//debug_current_and_destination(PSTR("done doing Z-Raise.")); //debug_current_and_destination(PSTR("done doing Z-Raise."));
destination[X_AXIS] = x_pos; // Move back to the starting position destination[X_AXIS] = g26_x_pos; // Move back to the starting position
destination[Y_AXIS] = y_pos; destination[Y_AXIS] = g26_y_pos;
//destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is //destination[Z_AXIS] = Z_CLEARANCE_BETWEEN_PROBES; // Keep the nozzle where it is
move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position move_to(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], 0); // Move back to the starting position
//debug_current_and_destination(PSTR("done doing X/Y move.")); //debug_current_and_destination(PSTR("done doing X/Y move."));
ubl.has_control_of_lcd_panel = false; // Give back control of the LCD Panel! has_control_of_lcd_panel = false; // Give back control of the LCD Panel!
if (!keep_heaters_on) { if (!g26_keep_heaters_on) {
#if HAS_TEMP_BED #if HAS_TEMP_BED
thermalManager.setTargetBed(0); thermalManager.setTargetBed(0);
#endif #endif
@ -391,14 +381,13 @@
} }
} }
float valid_trig_angle(float d) { float valid_trig_angle(float d) {
while (d > 360.0) d -= 360.0; while (d > 360.0) d -= 360.0;
while (d < 0.0) d += 360.0; while (d < 0.0) d += 360.0;
return d; return d;
} }
mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) { mesh_index_pair unified_bed_leveling::find_closest_circle_to_print(const float &X, const float &Y) {
float closest = 99999.99; float closest = 99999.99;
mesh_index_pair return_val; mesh_index_pair return_val;
@ -407,8 +396,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 = 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 = 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);
@ -417,7 +406,7 @@
// to let us find the closest circle to the start position. // to let us find the closest circle to the start position.
// But if this is not the case, add a small weighting to the // But if this is not the case, add a small weighting to the
// distance calculation to help it choose a better place to continue. // distance calculation to help it choose a better place to continue.
f += HYPOT(x_pos - mx, y_pos - my) / 15.0; f += HYPOT(g26_x_pos - mx, g26_y_pos - my) / 15.0;
// Add in the specified amount of Random Noise to our search // Add in the specified amount of Random Noise to our search
if (random_deviation > 1.0) if (random_deviation > 1.0)
@ -436,7 +425,7 @@
return return_val; return return_val;
} }
bool look_for_lines_to_connect() { bool unified_bed_leveling::look_for_lines_to_connect() {
float sx, sy, ex, ey; float sx, sy, ex, ey;
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
@ -454,16 +443,16 @@
// 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 = 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 = 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(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 (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) { if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
if (ubl.g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(", sy=", sy);
SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(") -> (ex=", ex);
@ -473,7 +462,7 @@
//debug_current_and_destination(PSTR("Connecting horizontal line.")); //debug_current_and_destination(PSTR("Connecting horizontal line."));
} }
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), g26_layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), g26_layer_height);
} }
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it
} }
@ -488,16 +477,16 @@
// 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 = 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 = 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(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);
if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) { if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) {
if (ubl.g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
SERIAL_ECHOPAIR(", sy=", sy); SERIAL_ECHOPAIR(", sy=", sy);
SERIAL_ECHOPAIR(") -> (ex=", ex); SERIAL_ECHOPAIR(") -> (ex=", ex);
@ -506,7 +495,7 @@
SERIAL_EOL; SERIAL_EOL;
debug_current_and_destination(PSTR("Connecting vertical line.")); debug_current_and_destination(PSTR("Connecting vertical line."));
} }
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), g26_layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), g26_layer_height);
} }
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped
} }
@ -518,7 +507,7 @@
return false; return false;
} }
void move_to(const float &x, const float &y, const float &z, const float &e_delta) { void unified_bed_leveling::move_to(const float &x, const float &y, const float &z, const float &e_delta) {
float feed_value; float feed_value;
static float last_z = -999.99; static float last_z = -999.99;
@ -540,10 +529,10 @@
} }
// Check if X or Y is involved in the movement. // Check if X or Y is involved in the movement.
// Yes: a 'normal' movement. No: a retract() or un_retract() // Yes: a 'normal' movement. No: a retract() or recover()
feed_value = has_xy_component ? PLANNER_XY_FEEDRATE() / 10.0 : planner.max_feedrate_mm_s[E_AXIS] / 1.5; feed_value = has_xy_component ? PLANNER_XY_FEEDRATE() / 10.0 : planner.max_feedrate_mm_s[E_AXIS] / 1.5;
if (ubl.g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to() feed_value for XY:", feed_value); if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to() feed_value for XY:", feed_value);
destination[X_AXIS] = x; destination[X_AXIS] = x;
destination[Y_AXIS] = y; destination[Y_AXIS] = y;
@ -556,16 +545,16 @@
} }
void retract_filament(float where[XYZE]) { void unified_bed_leveling::retract_filament(float where[XYZE]) {
if (!g26_retracted) { // Only retract if we are not already retracted! if (!g26_retracted) { // Only retract if we are not already retracted!
g26_retracted = true; g26_retracted = true;
move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], -1.0 * retraction_multiplier); move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], -1.0 * g26_retraction_multiplier);
} }
} }
void un_retract_filament(float where[XYZE]) { void unified_bed_leveling::recover_filament(float where[XYZE]) {
if (g26_retracted) { // Only un-retract if we are retracted. if (g26_retracted) { // Only un-retract if we are retracted.
move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], 1.2 * retraction_multiplier); move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], 1.2 * g26_retraction_multiplier);
g26_retracted = false; g26_retracted = false;
} }
} }
@ -585,7 +574,7 @@
* segment of a 'circle'. The time this requires is very short and is easily saved by the other * segment of a 'circle'. The time this requires is very short and is easily saved by the other
* cases where the optimization comes into play. * cases where the optimization comes into play.
*/ */
void print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) { void unified_bed_leveling::print_line_from_here_to_there(const float &sx, const float &sy, const float &sz, const float &ex, const float &ey, const float &ez) {
const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment const float dx_s = current_position[X_AXIS] - sx, // find our distance from the start of the actual line segment
dy_s = current_position[Y_AXIS] - sy, dy_s = current_position[Y_AXIS] - sy,
dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2 dist_start = HYPOT2(dx_s, dy_s), // We don't need to do a sqrt(), we can compare the distance^2
@ -613,9 +602,9 @@
move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion / un-Z bump move_to(sx, sy, sz, 0.0); // Get to the starting point with no extrusion / un-Z bump
const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier; const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier;
un_retract_filament(destination); recover_filament(destination);
move_to(ex, ey, ez, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion move_to(ex, ey, ez, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion
} }
@ -624,33 +613,33 @@
* parameters it made sense to turn them into static globals and get * parameters it made sense to turn them into static globals and get
* this code out of sight of the main routine. * this code out of sight of the main routine.
*/ */
bool parse_G26_parameters() { bool unified_bed_leveling::parse_G26_parameters() {
extrusion_multiplier = EXTRUSION_MULTIPLIER; g26_extrusion_multiplier = EXTRUSION_MULTIPLIER;
retraction_multiplier = RETRACTION_MULTIPLIER; g26_retraction_multiplier = RETRACTION_MULTIPLIER;
nozzle = NOZZLE; g26_nozzle = NOZZLE;
filament_diameter = FILAMENT; g26_filament_diameter = FILAMENT;
layer_height = LAYER_HEIGHT; g26_layer_height = LAYER_HEIGHT;
prime_length = PRIME_LENGTH; g26_prime_length = PRIME_LENGTH;
bed_temp = BED_TEMP; g26_bed_temp = BED_TEMP;
hotend_temp = HOTEND_TEMP; g26_hotend_temp = HOTEND_TEMP;
prime_flag = 0; g26_prime_flag = 0;
ooze_amount = code_seen('O') && code_has_value() ? code_value_linear_units() : OOZE_AMOUNT; g26_ooze_amount = code_seen('O') && code_has_value() ? code_value_linear_units() : OOZE_AMOUNT;
keep_heaters_on = code_seen('K') && code_value_bool(); g26_keep_heaters_on = code_seen('K') && code_value_bool();
continue_with_closest = code_seen('C') && code_value_bool(); g26_continue_with_closest = code_seen('C') && code_value_bool();
if (code_seen('B')) { if (code_seen('B')) {
bed_temp = code_value_temp_abs(); g26_bed_temp = code_value_temp_abs();
if (!WITHIN(bed_temp, 15, 140)) { if (!WITHIN(g26_bed_temp, 15, 140)) {
SERIAL_PROTOCOLLNPGM("?Specified bed temperature not plausible."); SERIAL_PROTOCOLLNPGM("?Specified bed temperature not plausible.");
return UBL_ERR; return UBL_ERR;
} }
} }
if (code_seen('L')) { if (code_seen('L')) {
layer_height = code_value_linear_units(); g26_layer_height = code_value_linear_units();
if (!WITHIN(layer_height, 0.0, 2.0)) { if (!WITHIN(g26_layer_height, 0.0, 2.0)) {
SERIAL_PROTOCOLLNPGM("?Specified layer height not plausible."); SERIAL_PROTOCOLLNPGM("?Specified layer height not plausible.");
return UBL_ERR; return UBL_ERR;
} }
@ -658,8 +647,8 @@
if (code_seen('Q')) { if (code_seen('Q')) {
if (code_has_value()) { if (code_has_value()) {
retraction_multiplier = code_value_float(); g26_retraction_multiplier = code_value_float();
if (!WITHIN(retraction_multiplier, 0.05, 15.0)) { if (!WITHIN(g26_retraction_multiplier, 0.05, 15.0)) {
SERIAL_PROTOCOLLNPGM("?Specified Retraction Multiplier not plausible."); SERIAL_PROTOCOLLNPGM("?Specified Retraction Multiplier not plausible.");
return UBL_ERR; return UBL_ERR;
} }
@ -671,8 +660,8 @@
} }
if (code_seen('S')) { if (code_seen('S')) {
nozzle = code_value_float(); g26_nozzle = code_value_float();
if (!WITHIN(nozzle, 0.1, 1.0)) { if (!WITHIN(g26_nozzle, 0.1, 1.0)) {
SERIAL_PROTOCOLLNPGM("?Specified nozzle size not plausible."); SERIAL_PROTOCOLLNPGM("?Specified nozzle size not plausible.");
return UBL_ERR; return UBL_ERR;
} }
@ -680,11 +669,11 @@
if (code_seen('P')) { if (code_seen('P')) {
if (!code_has_value()) if (!code_has_value())
prime_flag = -1; g26_prime_flag = -1;
else { else {
prime_flag++; g26_prime_flag++;
prime_length = code_value_linear_units(); g26_prime_length = code_value_linear_units();
if (!WITHIN(prime_length, 0.0, 25.0)) { if (!WITHIN(g26_prime_length, 0.0, 25.0)) {
SERIAL_PROTOCOLLNPGM("?Specified prime length not plausible."); SERIAL_PROTOCOLLNPGM("?Specified prime length not plausible.");
return UBL_ERR; return UBL_ERR;
} }
@ -692,21 +681,21 @@
} }
if (code_seen('F')) { if (code_seen('F')) {
filament_diameter = code_value_linear_units(); g26_filament_diameter = code_value_linear_units();
if (!WITHIN(filament_diameter, 1.0, 4.0)) { if (!WITHIN(g26_filament_diameter, 1.0, 4.0)) {
SERIAL_PROTOCOLLNPGM("?Specified filament size not plausible."); SERIAL_PROTOCOLLNPGM("?Specified filament size not plausible.");
return UBL_ERR; return UBL_ERR;
} }
} }
extrusion_multiplier *= sq(1.75) / sq(filament_diameter); // If we aren't using 1.75mm filament, we need to g26_extrusion_multiplier *= sq(1.75) / sq(g26_filament_diameter); // If we aren't using 1.75mm filament, we need to
// scale up or down the length needed to get the // scale up or down the length needed to get the
// same volume of filament // same volume of filament
extrusion_multiplier *= filament_diameter * sq(nozzle) / sq(0.3); // Scale up by nozzle size g26_extrusion_multiplier *= g26_filament_diameter * sq(g26_nozzle) / sq(0.3); // Scale up by nozzle size
if (code_seen('H')) { if (code_seen('H')) {
hotend_temp = code_value_temp_abs(); g26_hotend_temp = code_value_temp_abs();
if (!WITHIN(hotend_temp, 165, 280)) { if (!WITHIN(g26_hotend_temp, 165, 280)) {
SERIAL_PROTOCOLLNPGM("?Specified nozzle temperature not plausible."); SERIAL_PROTOCOLLNPGM("?Specified nozzle temperature not plausible.");
return UBL_ERR; return UBL_ERR;
} }
@ -723,9 +712,9 @@
return UBL_ERR; return UBL_ERR;
} }
x_pos = code_seen('X') ? code_value_linear_units() : current_position[X_AXIS]; g26_x_pos = code_seen('X') ? code_value_linear_units() : current_position[X_AXIS];
y_pos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS]; g26_y_pos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS];
if (!position_is_reachable_xy(x_pos, y_pos)) { if (!position_is_reachable_xy(g26_x_pos, g26_y_pos)) {
SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds."); SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds.");
return UBL_ERR; return UBL_ERR;
} }
@ -733,12 +722,12 @@
/** /**
* Wait until all parameters are verified before altering the state! * Wait until all parameters are verified before altering the state!
*/ */
ubl.state.active = !code_seen('D'); state.active = !code_seen('D');
return UBL_OK; return UBL_OK;
} }
bool exit_from_g26() { bool unified_bed_leveling::exit_from_g26() {
lcd_reset_alert_level(); lcd_reset_alert_level();
lcd_setstatuspgm(PSTR("Leaving G26")); lcd_setstatuspgm(PSTR("Leaving G26"));
while (ubl_lcd_clicked()) idle(); while (ubl_lcd_clicked()) idle();
@ -749,18 +738,18 @@
* Turn on the bed and nozzle heat and * Turn on the bed and nozzle heat and
* wait for them to get up to temperature. * wait for them to get up to temperature.
*/ */
bool turn_on_heaters() { bool unified_bed_leveling::turn_on_heaters() {
millis_t next; millis_t next;
#if HAS_TEMP_BED #if HAS_TEMP_BED
#if ENABLED(ULTRA_LCD) #if ENABLED(ULTRA_LCD)
if (bed_temp > 25) { if (g26_bed_temp > 25) {
lcd_setstatuspgm(PSTR("G26 Heating Bed."), 99); lcd_setstatuspgm(PSTR("G26 Heating Bed."), 99);
lcd_quick_feedback(); lcd_quick_feedback();
#endif #endif
ubl.has_control_of_lcd_panel = true; has_control_of_lcd_panel = true;
thermalManager.setTargetBed(bed_temp); thermalManager.setTargetBed(g26_bed_temp);
next = millis() + 5000UL; next = millis() + 5000UL;
while (abs(thermalManager.degBed() - bed_temp) > 3) { while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
if (ubl_lcd_clicked()) return exit_from_g26(); if (ubl_lcd_clicked()) return exit_from_g26();
if (PENDING(millis(), next)) { if (PENDING(millis(), next)) {
next = millis() + 5000UL; next = millis() + 5000UL;
@ -776,8 +765,8 @@
#endif #endif
// Start heating the nozzle and wait for it to reach temperature. // Start heating the nozzle and wait for it to reach temperature.
thermalManager.setTargetHotend(hotend_temp, 0); thermalManager.setTargetHotend(g26_hotend_temp, 0);
while (abs(thermalManager.degHotend(0) - hotend_temp) > 3) { while (abs(thermalManager.degHotend(0) - g26_hotend_temp) > 3) {
if (ubl_lcd_clicked()) return exit_from_g26(); if (ubl_lcd_clicked()) return exit_from_g26();
if (PENDING(millis(), next)) { if (PENDING(millis(), next)) {
next = millis() + 5000UL; next = millis() + 5000UL;
@ -798,19 +787,19 @@
/** /**
* Prime the nozzle if needed. Return true on error. * Prime the nozzle if needed. Return true on error.
*/ */
bool prime_nozzle() { bool unified_bed_leveling::prime_nozzle() {
float Total_Prime = 0.0; float Total_Prime = 0.0;
if (prime_flag == -1) { // The user wants to control how much filament gets purged if (g26_prime_flag == -1) { // The user wants to control how much filament gets purged
ubl.has_control_of_lcd_panel = true; has_control_of_lcd_panel = true;
lcd_setstatuspgm(PSTR("User-Controlled Prime"), 99); lcd_setstatuspgm(PSTR("User-Controlled Prime"), 99);
chirp_at_user(); chirp_at_user();
set_destination_to_current(); set_destination_to_current();
un_retract_filament(destination); // Make sure G26 doesn't think the filament is retracted(). recover_filament(destination); // Make sure G26 doesn't think the filament is retracted().
while (!ubl_lcd_clicked()) { while (!ubl_lcd_clicked()) {
chirp_at_user(); chirp_at_user();
@ -838,7 +827,7 @@
lcd_quick_feedback(); lcd_quick_feedback();
#endif #endif
ubl.has_control_of_lcd_panel = false; has_control_of_lcd_panel = false;
} }
else { else {
@ -847,7 +836,7 @@
lcd_quick_feedback(); lcd_quick_feedback();
#endif #endif
set_destination_to_current(); set_destination_to_current();
destination[E_AXIS] += prime_length; destination[E_AXIS] += g26_prime_length;
G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0); G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0);
stepper.synchronize(); stepper.synchronize();
set_destination_to_current(); set_destination_to_current();

12
Marlin/Marlin_main.cpp
View File

@ -3416,8 +3416,8 @@ inline void gcode_G7(
return; return;
} }
destination[X_AXIS] = hasI ? pgm_read_float(&ubl.mesh_index_to_xpos[ix]) : current_position[X_AXIS]; destination[X_AXIS] = hasI ? ubl.mesh_index_to_xpos(ix) : current_position[X_AXIS];
destination[Y_AXIS] = hasJ ? pgm_read_float(&ubl.mesh_index_to_ypos[iy]) : current_position[Y_AXIS]; destination[Y_AXIS] = hasJ ? ubl.mesh_index_to_ypos(iy) : current_position[Y_AXIS];
destination[Z_AXIS] = current_position[Z_AXIS]; //todo: perhaps add Z-move support? destination[Z_AXIS] = current_position[Z_AXIS]; //todo: perhaps add Z-move support?
destination[E_AXIS] = current_position[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS];
@ -8704,7 +8704,7 @@ void quickstop_stepper() {
const bool hasZ = code_seen('Z'), hasQ = !hasZ && code_seen('Q'); const bool hasZ = code_seen('Z'), hasQ = !hasZ && code_seen('Q');
if (hasC) { if (hasC) {
const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL, false); const mesh_index_pair location = ubl.find_closest_mesh_point_of_type(REAL, current_position[X_AXIS], current_position[Y_AXIS], USE_NOZZLE_AS_REFERENCE, NULL, false);
ix = location.x_index; ix = location.x_index;
iy = location.y_index; iy = location.y_index;
} }
@ -11467,7 +11467,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
const float fr_scaled = MMS_SCALED(feedrate_mm_s); const float fr_scaled = MMS_SCALED(feedrate_mm_s);
if (ubl.state.active) { if (ubl.state.active) {
ubl_line_to_destination_cartesian(fr_scaled, active_extruder); ubl.line_to_destination_cartesian(fr_scaled, active_extruder);
return true; return true;
} }
else else
@ -11612,14 +11612,14 @@ void prepare_move_to_destination() {
if ( if (
#if IS_KINEMATIC #if IS_KINEMATIC
#if UBL_DELTA #if UBL_DELTA
ubl_prepare_linear_move_to(destination, feedrate_mm_s) ubl.prepare_linear_move_to(destination, feedrate_mm_s)
#else #else
prepare_kinematic_move_to(destination) prepare_kinematic_move_to(destination)
#endif #endif
#elif ENABLED(DUAL_X_CARRIAGE) #elif ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx() prepare_move_to_destination_dualx()
#elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely) #elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
ubl_prepare_linear_move_to(destination, feedrate_mm_s) ubl.prepare_linear_move_to(destination, feedrate_mm_s)
#else #else
prepare_move_to_destination_cartesian() prepare_move_to_destination_cartesian()
#endif #endif

2
Marlin/fastio.h
View File

@ -58,7 +58,7 @@
#endif #endif
#ifndef _BV #ifndef _BV
#define _BV(PIN) (1 << PIN) #define _BV(PIN) (1UL << PIN)
#endif #endif
/** /**

8
Marlin/ubl.cpp
View File

@ -69,8 +69,8 @@
// 15 is the maximum nubmer of grid points supported + 1 safety margin for now, // 15 is the maximum nubmer of grid points supported + 1 safety margin for now,
// until determinism prevails // until determinism prevails
constexpr float unified_bed_leveling::mesh_index_to_xpos[16], constexpr float unified_bed_leveling::_mesh_index_to_xpos[16],
unified_bed_leveling::mesh_index_to_ypos[16]; unified_bed_leveling::_mesh_index_to_ypos[16];
bool unified_bed_leveling::g26_debug_flag = false, bool unified_bed_leveling::g26_debug_flag = false,
unified_bed_leveling::has_control_of_lcd_panel = false; unified_bed_leveling::has_control_of_lcd_panel = false;
@ -117,8 +117,8 @@
SERIAL_EOL; SERIAL_EOL;
} }
const float current_xi = ubl.get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0), const float current_xi = get_cell_index_x(current_position[X_AXIS] + (MESH_X_DIST) / 2.0),
current_yi = ubl.get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0); current_yi = get_cell_index_y(current_position[Y_AXIS] + (MESH_Y_DIST) / 2.0);
for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) { for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) { for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {

161
Marlin/ubl.h
View File

@ -53,30 +53,16 @@
// ubl_motion.cpp // ubl_motion.cpp
void debug_current_and_destination(const char * const title); void debug_current_and_destination(const char * const title);
void ubl_line_to_destination_cartesian(const float&, uint8_t);
bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate );
// ubl_G29.cpp // ubl_G29.cpp
enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
void dump(char * const str, const float &f);
void probe_entire_mesh(const float&, const float&, const bool, const bool, const bool);
float measure_business_card_thickness(float&);
mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
void shift_mesh_height();
void fine_tune_mesh(const float&, const float&, const bool);
bool g29_parameter_parsing();
void g29_eeprom_dump();
void g29_compare_current_mesh_to_stored_mesh();
// External references // External references
char *ftostr43sign(const float&, char); char *ftostr43sign(const float&, char);
bool ubl_lcd_clicked(); bool ubl_lcd_clicked();
void home_all_axes(); void home_all_axes();
void gcode_G26();
void gcode_G29();
extern uint8_t ubl_cnt; extern uint8_t ubl_cnt;
@ -101,26 +87,81 @@
static float last_specified_z; static float last_specified_z;
static int g29_verbose_level,
g29_phase_value,
g29_repetition_cnt,
g29_storage_slot,
g29_map_type,
g29_grid_size;
static bool g29_c_flag, g29_x_flag, g29_y_flag;
static float g29_x_pos, g29_y_pos,
g29_card_thickness,
g29_constant;
#if ENABLED(UBL_G26_MESH_VALIDATION)
static float g26_extrusion_multiplier,
g26_retraction_multiplier,
g26_nozzle,
g26_filament_diameter,
g26_prime_length,
g26_x_pos, g26_y_pos,
g26_ooze_amount,
g26_layer_height;
static int16_t g26_bed_temp,
g26_hotend_temp,
g26_repeats;
static int8_t g26_prime_flag;
static bool g26_continue_with_closest, g26_keep_heaters_on;
#endif
static float measure_point_with_encoder();
static float measure_business_card_thickness(float&);
static bool g29_parameter_parsing();
static void find_mean_mesh_height();
static void shift_mesh_height();
static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
static void g29_what_command();
static void g29_eeprom_dump();
static void g29_compare_current_mesh_to_stored_mesh();
static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
static void smart_fill_mesh();
#if ENABLED(UBL_G26_MESH_VALIDATION)
static bool exit_from_g26();
static bool parse_G26_parameters();
static void G26_line_to_destination(const float &feed_rate);
static mesh_index_pair find_closest_circle_to_print(const float&, const float&);
static bool look_for_lines_to_connect();
static bool turn_on_heaters();
static bool prime_nozzle();
static void retract_filament(float where[XYZE]);
static void recover_filament(float where[XYZE]);
static void print_line_from_here_to_there(const float&, const float&, const float&, const float&, const float&, const float&);
static void move_to(const float&, const float&, const float&, const float&);
#endif
public: public:
void echo_name(); static void echo_name();
void report_state(); static void report_state();
void find_mean_mesh_height(); static void save_ubl_active_state_and_disable();
void shift_mesh_height(); static void restore_ubl_active_state_and_leave();
void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest); static void display_map(const int);
void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3); static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); static void reset();
void save_ubl_active_state_and_disable(); static void invalidate();
void restore_ubl_active_state_and_leave(); static bool sanity_check();
void g29_what_command();
void g29_eeprom_dump(); static void G29() _O0; // O0 for no optimization
void g29_compare_current_mesh_to_stored_mesh(); static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560
void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
void smart_fill_mesh(); #if ENABLED(UBL_G26_MESH_VALIDATION)
void display_map(const int); static void G26();
void reset(); #endif
void invalidate();
bool sanity_check();
static ubl_state state; static ubl_state state;
@ -128,7 +169,7 @@
// 15 is the maximum nubmer of grid points supported + 1 safety margin for now, // 15 is the maximum nubmer of grid points supported + 1 safety margin for now,
// until determinism prevails // until determinism prevails
constexpr static float mesh_index_to_xpos[16] PROGMEM = { constexpr static float _mesh_index_to_xpos[16] PROGMEM = {
UBL_MESH_MIN_X + 0 * (MESH_X_DIST), UBL_MESH_MIN_X + 1 * (MESH_X_DIST), UBL_MESH_MIN_X + 0 * (MESH_X_DIST), UBL_MESH_MIN_X + 1 * (MESH_X_DIST),
UBL_MESH_MIN_X + 2 * (MESH_X_DIST), UBL_MESH_MIN_X + 3 * (MESH_X_DIST), UBL_MESH_MIN_X + 2 * (MESH_X_DIST), UBL_MESH_MIN_X + 3 * (MESH_X_DIST),
UBL_MESH_MIN_X + 4 * (MESH_X_DIST), UBL_MESH_MIN_X + 5 * (MESH_X_DIST), UBL_MESH_MIN_X + 4 * (MESH_X_DIST), UBL_MESH_MIN_X + 5 * (MESH_X_DIST),
@ -139,7 +180,7 @@
UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST) UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST)
}; };
constexpr static float mesh_index_to_ypos[16] PROGMEM = { constexpr static float _mesh_index_to_ypos[16] PROGMEM = {
UBL_MESH_MIN_Y + 0 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 1 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 0 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 1 * (MESH_Y_DIST),
UBL_MESH_MIN_Y + 2 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 3 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 2 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 3 * (MESH_Y_DIST),
UBL_MESH_MIN_Y + 4 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 5 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 4 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 5 * (MESH_Y_DIST),
@ -156,16 +197,16 @@
unified_bed_leveling(); unified_bed_leveling();
FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
int8_t get_cell_index_x(const float &x) { static int8_t get_cell_index_x(const float &x) {
const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX
} // position. But with this defined this way, it is possible } // position. But with this defined this way, it is possible
// to extrapolate off of this point even further out. Probably // to extrapolate off of this point even further out. Probably
// that is OK because something else should be keeping that from // that is OK because something else should be keeping that from
// happening and should not be worried about at this level. // happening and should not be worried about at this level.
int8_t get_cell_index_y(const float &y) { static int8_t get_cell_index_y(const float &y) {
const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX
} // position. But with this defined this way, it is possible } // position. But with this defined this way, it is possible
@ -173,12 +214,12 @@
// that is OK because something else should be keeping that from // that is OK because something else should be keeping that from
// happening and should not be worried about at this level. // happening and should not be worried about at this level.
int8_t find_closest_x_index(const float &x) { static int8_t find_closest_x_index(const float &x) {
const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
} }
int8_t find_closest_y_index(const float &y) { static int8_t find_closest_y_index(const float &y) {
const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
} }
@ -198,7 +239,7 @@
* It is fairly expensive with its 4 floating point additions and 2 floating point * It is fairly expensive with its 4 floating point additions and 2 floating point
* multiplications. * multiplications.
*/ */
FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1); return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
} }
@ -206,7 +247,7 @@
* z_correction_for_x_on_horizontal_mesh_line is an optimization for * z_correction_for_x_on_horizontal_mesh_line is an optimization for
* the rare occasion when a point lies exactly on a Mesh line (denoted by index yi). * the rare occasion when a point lies exactly on a Mesh line (denoted by index yi).
*/ */
inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) { inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) { if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") ); serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") );
SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0); SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
@ -217,7 +258,7 @@
return NAN; return NAN;
} }
const float xratio = (RAW_X_POSITION(lx0) - pgm_read_float(&mesh_index_to_xpos[x1_i])) * (1.0 / (MESH_X_DIST)), const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)),
z1 = z_values[x1_i][yi]; z1 = z_values[x1_i][yi];
return z1 + xratio * (z_values[x1_i + 1][yi] - z1); return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
@ -226,7 +267,7 @@
// //
// See comments above for z_correction_for_x_on_horizontal_mesh_line // See comments above for z_correction_for_x_on_horizontal_mesh_line
// //
inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) { inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) { if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") ); serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") );
SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0); SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0);
@ -237,7 +278,7 @@
return NAN; return NAN;
} }
const float yratio = (RAW_Y_POSITION(ly0) - pgm_read_float(&mesh_index_to_ypos[y1_i])) * (1.0 / (MESH_Y_DIST)), const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)),
z1 = z_values[xi][y1_i]; z1 = z_values[xi][y1_i];
return z1 + yratio * (z_values[xi][y1_i + 1] - z1); return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
@ -249,7 +290,7 @@
* Z-Height at both ends. Then it does a linear interpolation of these heights based * Z-Height at both ends. Then it does a linear interpolation of these heights based
* on the Y position within the cell. * on the Y position within the cell.
*/ */
float get_z_correction(const float &lx0, const float &ly0) { static float get_z_correction(const float &lx0, const float &ly0) {
const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)), const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
cy = get_cell_index_y(RAW_Y_POSITION(ly0)); cy = get_cell_index_y(RAW_Y_POSITION(ly0));
@ -268,16 +309,16 @@
} }
const float z1 = calc_z0(RAW_X_POSITION(lx0), const float z1 = calc_z0(RAW_X_POSITION(lx0),
pgm_read_float(&mesh_index_to_xpos[cx]), z_values[cx][cy], mesh_index_to_xpos(cx), z_values[cx][cy],
pgm_read_float(&mesh_index_to_xpos[cx + 1]), z_values[cx + 1][cy]); mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]);
const float z2 = calc_z0(RAW_X_POSITION(lx0), const float z2 = calc_z0(RAW_X_POSITION(lx0),
pgm_read_float(&mesh_index_to_xpos[cx]), z_values[cx][cy + 1], mesh_index_to_xpos(cx), z_values[cx][cy + 1],
pgm_read_float(&mesh_index_to_xpos[cx + 1]), z_values[cx + 1][cy + 1]); mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]);
float z0 = calc_z0(RAW_Y_POSITION(ly0), float z0 = calc_z0(RAW_Y_POSITION(ly0),
pgm_read_float(&mesh_index_to_ypos[cy]), z1, mesh_index_to_ypos(cy), z1,
pgm_read_float(&mesh_index_to_ypos[cy + 1]), z2); mesh_index_to_ypos(cy + 1), z2);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
@ -324,7 +365,7 @@
* Returns 0.0 if Z is past the specified 'Fade Height'. * Returns 0.0 if Z is past the specified 'Fade Height'.
*/ */
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
inline float fade_scaling_factor_for_z(const float &lz) { static inline float fade_scaling_factor_for_z(const float &lz) {
if (planner.z_fade_height == 0.0) return 1.0; if (planner.z_fade_height == 0.0) return 1.0;
static float fade_scaling_factor = 1.0; static float fade_scaling_factor = 1.0;
const float rz = RAW_Z_POSITION(lz); const float rz = RAW_Z_POSITION(lz);
@ -338,14 +379,24 @@
return fade_scaling_factor; return fade_scaling_factor;
} }
#else #else
inline float fade_scaling_factor_for_z(const float &lz) { FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { return 1.0; }
return 1.0;
}
#endif #endif
FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate);
static void line_to_destination_cartesian(const float &fr, uint8_t e);
}; // class unified_bed_leveling }; // class unified_bed_leveling
extern unified_bed_leveling ubl; extern unified_bed_leveling ubl;
#if ENABLED(UBL_G26_MESH_VALIDATION)
FORCE_INLINE void gcode_G26() { ubl.G26(); }
#endif
FORCE_INLINE void gcode_G29() { ubl.G29(); }
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL
#endif // UNIFIED_BED_LEVELING_H #endif // UNIFIED_BED_LEVELING_H

453
Marlin/ubl_G29.cpp
View File

File diff suppressed because it is too large

128
Marlin/ubl_motion.cpp
View File

@ -85,7 +85,7 @@
} }
void ubl_line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) { void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
/** /**
* Much of the nozzle movement will be within the same cell. So we will do as little computation * Much of the nozzle movement will be within the same cell. So we will do as little computation
* as possible to determine if this is the case. If this move is within the same cell, we will * as possible to determine if this is the case. If this move is within the same cell, we will
@ -104,19 +104,19 @@
destination[E_AXIS] destination[E_AXIS]
}; };
const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(start[X_AXIS])), const int cell_start_xi = get_cell_index_x(RAW_X_POSITION(start[X_AXIS])),
cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])), cell_start_yi = get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])),
cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(end[X_AXIS])), cell_dest_xi = get_cell_index_x(RAW_X_POSITION(end[X_AXIS])),
cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS])); cell_dest_yi = get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS]));
if (ubl.g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl_line_to_destination(xe=", end[X_AXIS]); SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL; SERIAL_EOL;
debug_current_and_destination(PSTR("Start of ubl_line_to_destination()")); debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
} }
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell, if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
@ -132,11 +132,11 @@
// Note: There is no Z Correction in this case. We are off the grid and don't know what // Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be. // a reasonable correction would be.
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + state.z_offset, end[E_AXIS], feed_rate, extruder);
set_current_to_destination(); set_current_to_destination();
if (ubl.g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl_line_to_destination()")); debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
return; return;
} }
@ -152,20 +152,20 @@
* 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]) - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)),
z1 = ubl.z_values[cell_dest_xi ][cell_dest_yi ] + xratio * z1 = 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 ]), (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
z2 = ubl.z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio * z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(ubl.z_values[cell_dest_xi + 1][cell_dest_yi + 1] - ubl.z_values[cell_dest_xi][cell_dest_yi + 1]); (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
// 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]) - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
float z0 = z1 + (z2 - z1) * yratio; float z0 = z1 + (z2 - z1) * yratio;
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]); z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
* If part of the Mesh is undefined, it will show up as NAN * If part of the Mesh is undefined, it will show up as NAN
@ -176,10 +176,10 @@
*/ */
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + state.z_offset, end[E_AXIS], feed_rate, extruder);
if (ubl.g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl_line_to_destination()")); debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
set_current_to_destination(); set_current_to_destination();
return; return;
@ -240,7 +240,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(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
@ -249,9 +249,9 @@
*/ */
const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi); float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]); z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
* If part of the Mesh is undefined, it will show up as NAN * If part of the Mesh is undefined, it will show up as NAN
@ -262,7 +262,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(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
@ -281,12 +281,12 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); planner._buffer_line(x, y, z_position + z0 + state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
if (ubl.g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl_line_to_destination()")); debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
// //
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done. // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
@ -311,12 +311,12 @@
// 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(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 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]); z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
* If part of the Mesh is undefined, it will show up as NAN * If part of the Mesh is undefined, it will show up as NAN
@ -327,7 +327,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(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
@ -346,12 +346,12 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); planner._buffer_line(x, y, z_position + z0 + state.z_offset, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
if (ubl.g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl_line_to_destination()")); debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE; goto FINAL_MOVE;
@ -377,8 +377,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(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(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.
@ -387,9 +387,9 @@
if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
// Yes! Crossing a Y Mesh Line next // Yes! Crossing a Y Mesh Line next
float z0 = ubl.z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi); float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]); z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
* If part of the Mesh is undefined, it will show up as NAN * If part of the Mesh is undefined, it will show up as NAN
@ -409,15 +409,15 @@
e_position = end[E_AXIS]; e_position = end[E_AXIS];
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); planner._buffer_line(x, next_mesh_line_y, z_position + z0 + state.z_offset, e_position, feed_rate, extruder);
current_yi += dyi; current_yi += dyi;
yi_cnt--; yi_cnt--;
} }
else { else {
// Yes! Crossing a X Mesh Line next // Yes! Crossing a X Mesh Line next
float z0 = ubl.z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag); float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag);
z0 *= ubl.fade_scaling_factor_for_z(end[Z_AXIS]); z0 *= fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
* If part of the Mesh is undefined, it will show up as NAN * If part of the Mesh is undefined, it will show up as NAN
@ -438,7 +438,7 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); planner._buffer_line(next_mesh_line_x, y, z_position + z0 + state.z_offset, e_position, feed_rate, extruder);
current_xi += dxi; current_xi += dxi;
xi_cnt--; xi_cnt--;
} }
@ -446,8 +446,8 @@
if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
} }
if (ubl.g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl_line_to_destination()")); debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE; goto FINAL_MOVE;
@ -502,7 +502,7 @@
* Returns true if the caller did NOT update current_position, otherwise false. * Returns true if the caller did NOT update current_position, otherwise false.
*/ */
static bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) { static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) // fail if moving outside reachable boundary if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate return true; // did not move, so current_position still accurate
@ -554,9 +554,9 @@
// Only compute leveling per segment if ubl active and target below z_fade_height. // Only compute leveling per segment if ubl active and target below z_fade_height.
if (!ubl.state.active || above_fade_height) { // no mesh leveling if (!state.active || above_fade_height) { // no mesh leveling
const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0; const float z_offset = state.active ? state.z_offset : 0.0;
float seg_dest[XYZE]; // per-segment destination, float seg_dest[XYZE]; // per-segment destination,
COPY_XYZE(seg_dest, current_position); // starting from current position COPY_XYZE(seg_dest, current_position); // starting from current position
@ -579,7 +579,7 @@
// Otherwise perform per-segment leveling // Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = ubl.fade_scaling_factor_for_z(ltarget[Z_AXIS]); const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
#endif #endif
float seg_dest[XYZE]; // per-segment destination, initialize to first segment float seg_dest[XYZE]; // per-segment destination, initialize to first segment
@ -591,7 +591,7 @@
float rx = RAW_X_POSITION(seg_dest[X_AXIS]), // assume raw vs logical coordinates shifted but not scaled. float rx = RAW_X_POSITION(seg_dest[X_AXIS]), // assume raw vs logical coordinates shifted but not scaled.
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
do { // for each mesh cell encountered during the move for(;;) { // for each mesh cell encountered during the move
// Compute mesh cell invariants that remain constant for all segments within cell. // Compute mesh cell invariants that remain constant for all segments within cell.
// Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter) // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
@ -606,19 +606,19 @@
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1); cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1); cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
const float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi ])), // 64 byte table lookup avoids mul+add const float x0 = pgm_read_float(&(mesh_index_to_xpos[cell_xi ])), // 64 byte table lookup avoids mul+add
y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi ])), // 64 byte table lookup avoids mul+add y0 = pgm_read_float(&(mesh_index_to_ypos[cell_yi ])), // 64 byte table lookup avoids mul+add
x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1])), // 64 byte table lookup avoids mul+add x1 = pgm_read_float(&(mesh_index_to_xpos[cell_xi+1])), // 64 byte table lookup avoids mul+add
y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1])); // 64 byte table lookup avoids mul+add y1 = pgm_read_float(&(mesh_index_to_ypos[cell_yi+1])); // 64 byte table lookup avoids mul+add
float cx = rx - x0, // cell-relative x float cx = rx - x0, // cell-relative x
cy = ry - y0, // cell-relative y cy = ry - y0, // cell-relative y
z_x0y0 = ubl.z_values[cell_xi ][cell_yi ], // z at lower left corner z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
z_x1y0 = ubl.z_values[cell_xi+1][cell_yi ], // z at upper left corner z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
z_x0y1 = ubl.z_values[cell_xi ][cell_yi+1], // z at lower right corner z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1]; // z at upper right corner z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating ubl.state.active (G29 A) if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating state.active (G29 A)
if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell, if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
@ -642,7 +642,7 @@
const float z_sxy0 = z_xmy0 * dx_seg, // per-segment adjustment to z_cxy0 const float z_sxy0 = z_xmy0 * dx_seg, // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg; // per-segment adjustment to z_cxym z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg; // per-segment adjustment to z_cxym
do { // for all segments within this mesh cell for(;;) { // for all segments within this mesh cell
float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy
@ -650,7 +650,7 @@
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
#endif #endif
z_cxcy += ubl.state.z_offset; // add fixed mesh offset from G29 Z z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z
if (--segments == 0) { // if this is last segment, use ltarget for exact if (--segments == 0) { // if this is last segment, use ltarget for exact
COPY_XYZE(seg_dest, ltarget); COPY_XYZE(seg_dest, ltarget);
@ -681,9 +681,9 @@
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0 z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
} while (true); // per-segment loop exits by break after last segment within cell, or by return on final segment } // segment loop
} while (true); // per-cell loop } // cell loop
} // end of function }
#endif // UBL_DELTA #endif // UBL_DELTA

Write Preview
Loading…
Cancel
Save