From b5711a99a255fe1ec6e8dbbbff69cfa34dd7c289 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 24 Mar 2017 00:53:37 -0500 Subject: [PATCH] Patches to bring UBL closer to compliance --- Marlin/Conditionals_post.h | 2 +- Marlin/G26_Mesh_Validation_Tool.cpp | 158 ++++------------ Marlin/Marlin_main.cpp | 52 +++--- Marlin/UBL.h | 162 +++++++++-------- Marlin/UBL_Bed_Leveling.cpp | 70 ++++---- Marlin/UBL_G29.cpp | 267 ++++++++++++---------------- Marlin/UBL_line_to_destination.cpp | 227 +++++++++++++---------- Marlin/configuration_store.cpp | 84 +++++---- Marlin/planner.cpp | 6 +- Marlin/planner.h | 8 +- 10 files changed, 493 insertions(+), 543 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 83f8059ea2..6594840808 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -669,7 +669,7 @@ #define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR)) #define HAS_ABL (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL)) - #define PLANNER_LEVELING ((HAS_ABL && DISABLED(AUTO_BED_LEVELING_UBL)) || ENABLED(MESH_BED_LEVELING)) + #define PLANNER_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) #if HAS_PROBING_PROCEDURE diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index d7dd605820..f7402667c1 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -24,25 +24,27 @@ * Marlin Firmware -- G26 - Mesh Validation Tool */ -#define EXTRUSION_MULTIPLIER 1.0 // This is too much clutter for the main Configuration.h file But -#define RETRACTION_MULTIPLIER 1.0 // some user have expressed an interest in being able to customize -#define NOZZLE 0.3 // these numbers for thier printer so they don't need to type all -#define FILAMENT 1.75 // the options every time they do a Mesh Validation Print. -#define LAYER_HEIGHT 0.2 -#define PRIME_LENGTH 10.0 // So, we put these number in an easy to find and change place. -#define BED_TEMP 60.0 -#define HOTEND_TEMP 205.0 -#define OOZE_AMOUNT 0.3 +#include "MarlinConfig.h" -#include "Marlin.h" -#include "Configuration.h" -#include "planner.h" -#include "stepper.h" -#include "temperature.h" -#include "UBL.h" -#include "ultralcd.h" +#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED) -#if ENABLED(AUTO_BED_LEVELING_UBL) + #include "Marlin.h" + #include "Configuration.h" + #include "planner.h" + #include "stepper.h" + #include "temperature.h" + #include "UBL.h" + #include "ultralcd.h" + + #define EXTRUSION_MULTIPLIER 1.0 // This is too much clutter for the main Configuration.h file But + #define RETRACTION_MULTIPLIER 1.0 // some user have expressed an interest in being able to customize + #define NOZZLE 0.3 // these numbers for thier printer so they don't need to type all + #define FILAMENT 1.75 // the options every time they do a Mesh Validation Print. + #define LAYER_HEIGHT 0.2 + #define PRIME_LENGTH 10.0 // So, we put these number in an easy to find and change place. + #define BED_TEMP 60.0 + #define HOTEND_TEMP 205.0 + #define OOZE_AMOUNT 0.3 #define SIZE_OF_INTERSECTION_CIRCLES 5 #define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle. This number should be @@ -50,64 +52,64 @@ /** * Roxy's G26 Mesh Validation Tool - * + * * G26 Is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System. * In order to fully utilize and benefit from the Marlin Unified Bed Leveling System an accurate Mesh must * be defined. G29 is designed to allow the user to quickly validate the correctness of her Mesh. It will * first heat the bed and nozzle. It will then print lines and circles along the Mesh Cell boundaries and * the intersections of those lines (respectively). - * + * * This action allows the user to immediately see where the Mesh is properly defined and where it needs to * be edited. The command will generate the Mesh lines closest to the nozzle's starting position. Alternatively * the user can specify the X and Y position of interest with command parameters. This allows the user to * focus on a particular area of the Mesh where attention is needed. - * + * * B # Bed Set the Bed Temperature. If not specified, a default of 60 C. will be assumed. - * + * * C Current When searching for Mesh Intersection points to draw, use the current nozzle location * as the base for any distance comparison. - * + * * D Disable Disable the Unified Bed Leveling System. In the normal case the user is invoking this * command to see how well a Mesh as been adjusted to match a print surface. In order to do * this the Unified Bed Leveling System is turned on by the G26 command. The D parameter * alters the command's normal behaviour and disables the Unified Bed Leveling System even if * it is on. - * + * * H # Hotend Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed. - * + * * F # Filament Used to specify the diameter of the filament being used. If not specified * 1.75mm filament is assumed. If you are not getting acceptable results by using the * 'correct' numbers, you can scale this number up or down a little bit to change the amount * of filament that is being extruded during the printing of the various lines on the bed. - * + * * K Keep-On Keep the heaters turned on at the end of the command. - * + * * L # Layer Layer height. (Height of nozzle above bed) If not specified .20mm will be used. - * + * * Q # Multiplier Retraction Multiplier. Normally not needed. Retraction defaults to 1.0mm and * un-retraction is at 1.2mm These numbers will be scaled by the specified amount - * + * * N # Nozzle Used to control the size of nozzle diameter. If not specified, a .4mm nozzle is assumed. - * + * * O # Ooooze How much your nozzle will Ooooze filament while getting in position to print. This * is over kill, but using this parameter will let you get the very first 'cicle' perfect * so you have a trophy to peel off of the bed and hang up to show how perfectly you have your * Mesh calibrated. If not specified, a filament length of .3mm is assumed. - * + * * P # Prime Prime the nozzle with specified length of filament. If this parameter is not * given, no prime action will take place. If the parameter specifies an amount, that much * will be purged before continuing. If no amount is specified the command will start * purging filament until the user provides an LCD Click and then it will continue with * printing the Mesh. You can carefully remove the spent filament with a needle nose * pliers while holding the LCD Click wheel in a depressed state. - * + * * R # Random Randomize the order that the circles are drawn on the bed. The search for the closest * undrawn cicle is still done. But the distance to the location for each circle has a * random number of the size specified added to it. Specifying R50 will give an interesting * deviation from the normal behaviour on a 10 x 10 Mesh. - * + * * X # X coordinate Specify the starting location of the drawing activity. - * + * * Y # Y coordinate Specify the starting location of the drawing activity. */ @@ -156,7 +158,6 @@ float valid_trig_angle(float); mesh_index_pair find_closest_circle_to_print(float, float); - void debug_current_and_destination(char *title); void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t); //uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF); /* needed for the old mesh_buffer_line() routine */ @@ -172,19 +173,8 @@ int8_t prime_flag = 0; - bool keep_heaters_on = false; - - bool g26_debug_flag = false; - - /** - * These support functions allow the use of large bit arrays of flags that take very - * little RAM. Currently they are limited to being 16x16 in size. Changing the declaration - * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed - * in the future. - */ - void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); } - void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); } - bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); } + bool keep_heaters_on = false, + g26_debug_flag = false; /** * G26: Mesh Validation Pattern generation. @@ -544,78 +534,6 @@ } } - void debug_current_and_destination(char *title) { - float dx, dy, de, xy_dist, fpmm; - - // if the title message starts with a '!' it is so important, we are going to - // ignore the status of the g26_debug_flag - if (*title != '!' && !g26_debug_flag) return; - - dx = current_position[X_AXIS] - destination[X_AXIS]; - dy = current_position[Y_AXIS] - destination[Y_AXIS]; - de = destination[E_AXIS] - current_position[E_AXIS]; - if (de == 0.0) return; - - xy_dist = HYPOT(dx, dy); - if (xy_dist == 0.0) { - return; - //SERIAL_ECHOPGM(" FPMM="); - //fpmm = de; - //SERIAL_PROTOCOL_F(fpmm, 6); - } - else { - SERIAL_ECHOPGM(" fpmm="); - fpmm = de / xy_dist; - SERIAL_ECHO_F(fpmm, 6); - } - - SERIAL_ECHOPGM(" current=( "); - SERIAL_ECHO_F(current_position[X_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[Y_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[Z_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[E_AXIS], 6); - SERIAL_ECHOPGM(" ) destination=( "); - if (current_position[X_AXIS] == destination[X_AXIS]) - SERIAL_ECHOPGM("-------------"); - else - SERIAL_ECHO_F(destination[X_AXIS], 6); - - SERIAL_ECHOPGM(", "); - - if (current_position[Y_AXIS] == destination[Y_AXIS]) - SERIAL_ECHOPGM("-------------"); - else - SERIAL_ECHO_F(destination[Y_AXIS], 6); - - SERIAL_ECHOPGM(", "); - - if (current_position[Z_AXIS] == destination[Z_AXIS]) - SERIAL_ECHOPGM("-------------"); - else - SERIAL_ECHO_F(destination[Z_AXIS], 6); - - SERIAL_ECHOPGM(", "); - - if (current_position[E_AXIS] == destination[E_AXIS]) - SERIAL_ECHOPGM("-------------"); - else - SERIAL_ECHO_F(destination[E_AXIS], 6); - - SERIAL_ECHOPGM(" ) "); - SERIAL_ECHO(title); - SERIAL_EOL; - - SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66. Right Switch is on pin 65 - - //if (been_to_2_6) { - //while ((digitalRead(66) & 0x01) != 0) - // idle(); - //} - } - void move_to(const float &x, const float &y, const float &z, const float &e_delta) { float feed_value; static float last_z = -999.99; @@ -1002,4 +920,4 @@ return UBL_OK; } -#endif // AUTO_BED_LEVELING_UBL +#endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 5edf898a8d..089d0fb4b2 100755 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -233,10 +233,6 @@ #include "duration_t.h" #include "types.h" -#if ENABLED(AUTO_BED_LEVELING_UBL) - #include "UBL.h" -#endif - #if HAS_ABL #include "vector_3.h" #if ENABLED(AUTO_BED_LEVELING_LINEAR) @@ -301,7 +297,13 @@ #endif #if ENABLED(AUTO_BED_LEVELING_UBL) + #include "UBL.h" unified_bed_leveling ubl; +#define UBL_MESH_VALID !( z_values[0][0] == z_values[0][1] && z_values[0][1] == z_values[0][2] \ + && z_values[1][0] == z_values[1][1] && z_values[1][1] == z_values[1][2] \ + && z_values[2][0] == z_values[2][1] && z_values[2][1] == z_values[2][2] \ + && z_values[0][0] == 0 && z_values[1][0] == 0 && z_values[2][0] == 0 \ + || isnan(z_values[0][0])) #endif bool Running = true; @@ -2266,7 +2268,7 @@ static void clean_up_after_endstop_or_probe_move() { #endif // HAS_BED_PROBE -#if PLANNER_LEVELING || ENABLED(AUTO_BED_LEVELING_UBL) +#if PLANNER_LEVELING /** * Turn bed leveling on or off, fixing the current * position as-needed. @@ -2309,7 +2311,8 @@ static void clean_up_after_endstop_or_probe_move() { planner.unapply_leveling(current_position); } #elif ENABLED(AUTO_BED_LEVELING_UBL) - ubl.state.active = enable; + ubl.state.active = enable; + //set_current_from_steppers_for_axis(Z_AXIS); #endif } @@ -3481,11 +3484,6 @@ inline void gcode_G4() { * */ inline void gcode_G28() { - #if ENABLED(AUTO_BED_LEVELING_UBL) - bool bed_leveling_state_at_entry=0; - bed_leveling_state_at_entry = ubl.state.active; - set_bed_leveling_enabled(false); - #endif #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { @@ -3498,7 +3496,10 @@ inline void gcode_G28() { stepper.synchronize(); // Disable the leveling matrix before homing - #if PLANNER_LEVELING || ENABLED(MESH_BED_LEVELING) + #if PLANNER_LEVELING + #if ENABLED(AUTO_BED_LEVELING_UBL) + const bool bed_leveling_state_at_entry = ubl.state.active; + #endif set_bed_leveling_enabled(false); #endif @@ -5305,6 +5306,18 @@ inline void gcode_M42() { #endif // Z_MIN_PROBE_REPEATABILITY_TEST +#if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED) + + inline void gcode_M49() { + SERIAL_PROTOCOLPGM("UBL Debug Flag turned "); + if ((g26_debug_flag = !g26_debug_flag)) + SERIAL_PROTOCOLLNPGM("on."); + else + SERIAL_PROTOCOLLNPGM("off."); + } + +#endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED + /** * M75: Start print timer */ @@ -8512,7 +8525,7 @@ void process_next_command() { break; #endif // INCH_MODE_SUPPORT - #if ENABLED(AUTO_BED_LEVELING_UBL) + #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED) case 26: // G26: Mesh Validation Pattern generation gcode_G26(); break; @@ -8644,16 +8657,11 @@ void process_next_command() { break; #endif // Z_MIN_PROBE_REPEATABILITY_TEST - #if ENABLED(AUTO_BED_LEVELING_UBL) + #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED) case 49: // M49: Turn on or off g26_debug_flag for verbose output - if (g26_debug_flag) { - SERIAL_PROTOCOLPGM("UBL Debug Flag turned off.\n"); - g26_debug_flag = 0; } - else { - SERIAL_PROTOCOLPGM("UBL Debug Flag turned on.\n"); - g26_debug_flag++; } + gcode_M49(); break; - #endif // Z_MIN_PROBE_REPEATABILITY_TEST + #endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED case 75: // M75: Start print timer gcode_M75(); break; @@ -9547,7 +9555,7 @@ void get_cartesian_from_steppers() { */ void set_current_from_steppers_for_axis(const AxisEnum axis) { get_cartesian_from_steppers(); - #if PLANNER_LEVELING + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) planner.unapply_leveling(cartes); #endif if (axis == ALL_AXES) diff --git a/Marlin/UBL.h b/Marlin/UBL.h index 11d7c2fd91..ffb96a9f6f 100644 --- a/Marlin/UBL.h +++ b/Marlin/UBL.h @@ -22,6 +22,7 @@ #include "Marlin.h" #include "math.h" +#include "vector_3.h" #ifndef UNIFIED_BED_LEVELING_H #define UNIFIED_BED_LEVELING_H @@ -32,32 +33,29 @@ #define UBL_ERR true typedef struct { - int x_index, y_index; - float distance; // Not always used. But when populated, it is the distance - // from the search location + int8_t x_index, y_index; + float distance; // When populated, the distance from the search location } mesh_index_pair; - typedef struct { double dx, dy, dz; } vector; - enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; bool axis_unhomed_error(bool, bool, bool); - void dump(char *str, float f); + void dump(char * const str, const float &f); bool ubl_lcd_clicked(); - void probe_entire_mesh(float, float, bool, bool); + void probe_entire_mesh(const float&, const float&, const bool, const bool); + void debug_current_and_destination(char *title); void ubl_line_to_destination(const float&, const float&, const float&, const float&, const float&, uint8_t); - void manually_probe_remaining_mesh(float, float, float, float, bool); - vector tilt_mesh_based_on_3pts(float, float, float); - void new_set_bed_level_equation_3pts(float, float, float); - float measure_business_card_thickness(float); - mesh_index_pair find_closest_mesh_point_of_type(MeshPointType, float, float, bool, unsigned int[16]); + void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool); + vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&); + float measure_business_card_thickness(const float&); + mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16]); void find_mean_mesh_height(); void shift_mesh_height(); bool g29_parameter_parsing(); void g29_what_command(); void g29_eeprom_dump(); void g29_compare_current_mesh_to_stored_mesh(); - void fine_tune_mesh(float, float, bool); + void fine_tune_mesh(const float&, const float&, const bool); void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y); void bit_set(uint16_t bits[16], uint8_t x, uint8_t y); bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y); @@ -83,7 +81,11 @@ #define MESH_X_DIST ((float(UBL_MESH_MAX_X) - float(UBL_MESH_MIN_X)) / (float(UBL_MESH_NUM_X_POINTS) - 1.0)) #define MESH_Y_DIST ((float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / (float(UBL_MESH_NUM_Y_POINTS) - 1.0)) - extern bool g26_debug_flag; + #if ENABLED(UBL_MESH_EDIT_ENABLED) + extern bool g26_debug_flag; + #else + constexpr bool g26_debug_flag = false; + #endif extern float last_specified_z; extern float fade_scaling_factor_for_current_height; extern float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS]; @@ -103,12 +105,15 @@ mesh_x_max = UBL_MESH_MAX_X, mesh_y_max = UBL_MESH_MAX_Y, mesh_x_dist = MESH_X_DIST, - mesh_y_dist = MESH_Y_DIST, - g29_correction_fade_height = 10.0, - g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating - // point divide. So, we keep this number in both forms. The first - // is for the user. The second one is the one that is actually used - // again and again and again during the correction calculations. + mesh_y_dist = MESH_Y_DIST; + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + float g29_correction_fade_height = 10.0, + g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating + // point divide. So, we keep this number in both forms. The first + // is for the user. The second one is the one that is actually used + // again and again and again during the correction calculations. + #endif unsigned char padding[24]; // This is just to allow room to add state variables without // changing the location of data structures in the EEPROM. @@ -122,45 +127,45 @@ unified_bed_leveling(); // ~unified_bed_leveling(); // No destructor because this object never goes away! - void display_map(int); + void display_map(const int); void reset(); void invalidate(); void store_state(); void load_state(); - void store_mesh(int); - void load_mesh(int); + void store_mesh(const int16_t); + void load_mesh(const int16_t); bool sanity_check(); - FORCE_INLINE float map_x_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_X) + (((float) MESH_X_DIST) * (float) i); }; - FORCE_INLINE float map_y_index_to_bed_location(int8_t i){ return ((float) UBL_MESH_MIN_Y) + (((float) MESH_Y_DIST) * (float) i); }; + FORCE_INLINE static float map_x_index_to_bed_location(const int8_t i) { return ((float) UBL_MESH_MIN_X) + (((float) MESH_X_DIST) * (float) i); }; + FORCE_INLINE static float map_y_index_to_bed_location(const int8_t i) { return ((float) UBL_MESH_MIN_Y) + (((float) MESH_Y_DIST) * (float) i); }; - void set_z(const int8_t px, const int8_t py, const float z) { z_values[px][py] = z; } + FORCE_INLINE 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(float x) { - int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); + static int8_t get_cell_index_x(const float &x) { + const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); return constrain(cx, 0, (UBL_MESH_NUM_X_POINTS) - 1); // -1 is appropriate if we want all movement to the X_MAX } // position. But with this defined this way, it is possible // to extrapolate off of this point even further out. Probably // that is OK because something else should be keeping that from // happening and should not be worried about at this level. - int8_t get_cell_index_y(float y) { - int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); + static int8_t get_cell_index_y(const float &y) { + const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST)); return constrain(cy, 0, (UBL_MESH_NUM_Y_POINTS) - 1); // -1 is appropriate if we want all movement to the Y_MAX } // position. But with this defined this way, it is possible // to extrapolate off of this point even further out. Probably // that is OK because something else should be keeping that from // happening and should not be worried about at this level. - int8_t find_closest_x_index(float x) { - int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); + 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)); return (px >= 0 && px < (UBL_MESH_NUM_X_POINTS)) ? px : -1; } - int8_t find_closest_y_index(float y) { - int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); + 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)); return (py >= 0 && py < (UBL_MESH_NUM_Y_POINTS)) ? py : -1; } @@ -174,14 +179,14 @@ * |<---delta_a---------->| * * calc_z0 is the basis for all the Mesh Based correction. It is used to - * find the expected Z Height at a position between two known Z-Height locations + * find the expected Z Height at a position between two known Z-Height locations. * - * It is farly 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. */ - inline float calc_z0(float a0, float a1, float z1, float a2, float z2) { - float delta_z = (z2 - z1); - float delta_a = (a0 - a1) / (a2 - a1); + static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { + const float delta_z = (z2 - z1), + delta_a = (a0 - a1) / (a2 - a1); return z1 + delta_a * delta_z; } @@ -193,7 +198,7 @@ * the X index of the x0 intersection available and we don't want to perform any extra floating * point operations. */ - inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(float x0, int x1_i, int yi) { + inline float get_z_correction_along_horizontal_mesh_line_at_specific_X(const float &x0, const int x1_i, const int yi) { if (x1_i < 0 || yi < 0 || x1_i >= UBL_MESH_NUM_X_POINTS || yi >= UBL_MESH_NUM_Y_POINTS) { SERIAL_ECHOPAIR("? in get_z_correction_along_horizontal_mesh_line_at_specific_X(x0=", x0); SERIAL_ECHOPAIR(",x1_i=", x1_i); @@ -203,18 +208,18 @@ return NAN; } - const float a0ma1diva2ma1 = (x0 - mesh_index_to_x_location[x1_i]) * (1.0 / (MESH_X_DIST)), + const float xratio = (RAW_X_POSITION(x0) - mesh_index_to_x_location[x1_i]) * (1.0 / (MESH_X_DIST)), z1 = z_values[x1_i][yi], z2 = z_values[x1_i + 1][yi], dz = (z2 - z1); - return z1 + a0ma1diva2ma1 * dz; + return z1 + xratio * dz; } // // See comments above for get_z_correction_along_horizontal_mesh_line_at_specific_X // - inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(float y0, int xi, int y1_i) { + inline float get_z_correction_along_vertical_mesh_line_at_specific_Y(const float &y0, const int xi, const int y1_i) { if (xi < 0 || y1_i < 0 || xi >= UBL_MESH_NUM_X_POINTS || y1_i >= UBL_MESH_NUM_Y_POINTS) { SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_X(y0=", y0); SERIAL_ECHOPAIR(", x1_i=", xi); @@ -224,12 +229,12 @@ return NAN; } - const float a0ma1diva2ma1 = (y0 - mesh_index_to_y_location[y1_i]) * (1.0 / (MESH_Y_DIST)), + const float yratio = (RAW_Y_POSITION(y0) - mesh_index_to_y_location[y1_i]) * (1.0 / (MESH_Y_DIST)), z1 = z_values[xi][y1_i], z2 = z_values[xi][y1_i + 1], dz = (z2 - z1); - return z1 + a0ma1diva2ma1 * dz; + return z1 + yratio * dz; } /** @@ -238,9 +243,9 @@ * Z-Height at both ends. Then it does a linear interpolation of these heights based * on the Y position within the cell. */ - float get_z_correction(float x0, float y0) { - int8_t cx = get_cell_index_x(x0), - cy = get_cell_index_y(y0); + float get_z_correction(const float &x0, const float &y0) const { + const int8_t cx = get_cell_index_x(RAW_X_POSITION(x0)), + cy = get_cell_index_y(RAW_Y_POSITION(y0)); if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) { @@ -256,15 +261,15 @@ return 0.0; // this used to return state.z_offset } - float z1 = calc_z0(x0, - map_x_index_to_bed_location(cx), z_values[cx][cy], - map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]); - float z2 = calc_z0(x0, - map_x_index_to_bed_location(cx), z_values[cx][cy + 1], - map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]); - float z0 = calc_z0(y0, - map_y_index_to_bed_location(cy), z1, - map_y_index_to_bed_location(cy + 1), z2); + const float z1 = calc_z0(RAW_X_POSITION(x0), + map_x_index_to_bed_location(cx), z_values[cx][cy], + map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]), + z2 = calc_z0(RAW_X_POSITION(x0), + map_x_index_to_bed_location(cx), z_values[cx][cy + 1], + map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]); + float z0 = calc_z0(RAW_Y_POSITION(y0), + map_y_index_to_bed_location(cy), z1, + map_y_index_to_bed_location(cy + 1), z2); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(MESH_ADJUST)) { @@ -308,27 +313,36 @@ * factor is going to be the same as the last time the function calculated a value. If so, it just * returns it. * - * If it must do a calcuation, it will return a scaling factor of 0.0 if the UBL System is not active - * or if the current Z Height is past the specified 'Fade Height' + * It returns a scaling factor of 1.0 if UBL is inactive. + * It returns a scaling factor of 0.0 if Z is past the specified 'Fade Height' */ - FORCE_INLINE float fade_scaling_factor_for_z(float current_z) { - #ifndef ENABLE_LEVELING_FADE_HEIGHT // if turned off, just return 0.000 Note that we assume the - return 0.000; // compiler will do 'Dead Code' elimination so there is no need - #endif // for an #else clause here. - if (last_specified_z == current_z) - return fade_scaling_factor_for_current_height; + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - last_specified_z = current_z; - fade_scaling_factor_for_current_height = - state.active && current_z < state.g29_correction_fade_height - ? 1.0 - (current_z * state.g29_fade_height_multiplier) - : 0.0; - return fade_scaling_factor_for_current_height; - } - }; + FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) const { + const float rz = RAW_Z_POSITION(lz); + if (last_specified_z != rz) { + last_specified_z = rz; + fade_scaling_factor_for_current_height = + state.active && rz < state.g29_correction_fade_height + ? 1.0 - (rz * state.g29_fade_height_multiplier) + : 0.0; + } + return fade_scaling_factor_for_current_height; + } + + #else + + static constexpr float fade_scaling_factor_for_z(const float &lz) { UNUSED(lz); return 1.0; } + + #endif + + }; // class unified_bed_leveling extern unified_bed_leveling ubl; extern int ubl_eeprom_start; -#endif // AUTO_BED_LEVELING_UBL + #define UBL_LAST_EEPROM_INDEX (E2END - sizeof(unified_bed_leveling::state)) + + #endif // AUTO_BED_LEVELING_UBL + #endif // UNIFIED_BED_LEVELING_H diff --git a/Marlin/UBL_Bed_Leveling.cpp b/Marlin/UBL_Bed_Leveling.cpp index 1494c9b03d..cc4875a443 100644 --- a/Marlin/UBL_Bed_Leveling.cpp +++ b/Marlin/UBL_Bed_Leveling.cpp @@ -24,9 +24,20 @@ #include "math.h" #if ENABLED(AUTO_BED_LEVELING_UBL) + #include "UBL.h" #include "hex_print_routines.h" + /** + * These support functions allow the use of large bit arrays of flags that take very + * little RAM. Currently they are limited to being 16x16 in size. Changing the declaration + * to unsigned long will allow us to go to 32x32 if higher resolution Mesh's are needed + * in the future. + */ + void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y) { CBI(bits[y], x); } + void bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { SBI(bits[y], x); } + bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y) { return TEST(bits[y], x); } + /** * These variables used to be declared inside the unified_bed_leveling class. We are going to * still declare them within the .cpp file for bed leveling. But there is only one instance of @@ -51,36 +62,36 @@ } void unified_bed_leveling::store_state() { - int k = E2END - sizeof(ubl.state); - eeprom_write_block((void *)&ubl.state, (void *)k, sizeof(ubl.state)); + const uint16_t i = UBL_LAST_EEPROM_INDEX; + eeprom_write_block((void *)&ubl.state, (void *)i, sizeof(state)); } void unified_bed_leveling::load_state() { - int k = E2END - sizeof(ubl.state); - eeprom_read_block((void *)&ubl.state, (void *)k, sizeof(ubl.state)); + const uint16_t i = UBL_LAST_EEPROM_INDEX; + eeprom_read_block((void *)&ubl.state, (void *)i, sizeof(state)); if (sanity_check()) SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n"); - /** - * These lines can go away in a few weeks. They are just - * to make sure people updating thier firmware won't be using - * an incomplete Bed_Leveling.state structure. For speed - * we now multiply by the inverse of the Fade Height instead of - * dividing by it. Soon... all of the old structures will be - * updated, but until then, we try to ease the transition - * for our Beta testers. - */ - if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) { - ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height; - store_state(); - } - + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + /** + * These lines can go away in a few weeks. They are just + * to make sure people updating thier firmware won't be using + * an incomplete Bed_Leveling.state structure. For speed + * we now multiply by the inverse of the Fade Height instead of + * dividing by it. Soon... all of the old structures will be + * updated, but until then, we try to ease the transition + * for our Beta testers. + */ + if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) { + ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height; + store_state(); + } + #endif } - void unified_bed_leveling::load_mesh(int m) { - int k = E2END - sizeof(ubl.state), - j = (k - ubl_eeprom_start) / sizeof(z_values); + void unified_bed_leveling::load_mesh(const int16_t m) { + int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values); if (m == -1) { SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n"); @@ -93,7 +104,7 @@ return; } - j = k - (m + 1) * sizeof(z_values); + j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values); eeprom_read_block((void *)&z_values , (void *)j, sizeof(z_values)); SERIAL_PROTOCOLPGM("Mesh loaded from slot "); @@ -103,23 +114,22 @@ SERIAL_EOL; } - void unified_bed_leveling:: store_mesh(int m) { - int k = E2END - sizeof(state), - j = (k - ubl_eeprom_start) / sizeof(z_values); + void unified_bed_leveling::store_mesh(const int16_t m) { + int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values); if (m < 0 || m >= j || ubl_eeprom_start <= 0) { SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n"); SERIAL_PROTOCOL(m); SERIAL_PROTOCOLLNPGM(" mesh slots available.\n"); SERIAL_PROTOCOLLNPAIR("E2END : ", E2END); - SERIAL_PROTOCOLLNPAIR("k : ", k); + SERIAL_PROTOCOLLNPAIR("k : ", (int)UBL_LAST_EEPROM_INDEX); SERIAL_PROTOCOLLNPAIR("j : ", j); SERIAL_PROTOCOLLNPAIR("m : ", m); SERIAL_EOL; return; } - j = k - (m + 1) * sizeof(z_values); + j = UBL_LAST_EEPROM_INDEX - (m + 1) * sizeof(z_values); eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values)); SERIAL_PROTOCOLPGM("Mesh saved in slot "); @@ -151,7 +161,7 @@ z_values[x][y] = NAN; } - void unified_bed_leveling::display_map(int map_type) { + void unified_bed_leveling::display_map(const int map_type) { float f, current_xi, current_yi; int8_t i, j; UNUSED(map_type); @@ -287,9 +297,7 @@ error_flag++; } - const int k = E2END - sizeof(ubl.state), - j = (k - ubl_eeprom_start) / sizeof(z_values); - + const int j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values); if (j < 1) { SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n"); error_flag++; diff --git a/Marlin/UBL_G29.cpp b/Marlin/UBL_G29.cpp index 74479bd5ce..0c74f67773 100644 --- a/Marlin/UBL_G29.cpp +++ b/Marlin/UBL_G29.cpp @@ -20,12 +20,14 @@ * */ -#include "Marlin.h" +#include "MarlinConfig.h" + #if ENABLED(AUTO_BED_LEVELING_UBL) //#include "vector_3.h" //#include "qr_solve.h" #include "UBL.h" + #include "Marlin.h" #include "hex_print_routines.h" #include "configuration_store.h" #include "planner.h" @@ -49,12 +51,13 @@ #define DEPLOY_PROBE() set_probe_deployed(true) #define STOW_PROBE() set_probe_deployed(false) bool ProbeStay = true; - float ubl_3_point_1_X = UBL_PROBE_PT_1_X; - float ubl_3_point_1_Y = UBL_PROBE_PT_1_Y; - float ubl_3_point_2_X = UBL_PROBE_PT_2_X; - float ubl_3_point_2_Y = UBL_PROBE_PT_2_Y; - float ubl_3_point_3_X = UBL_PROBE_PT_3_X; - float ubl_3_point_3_Y = UBL_PROBE_PT_3_Y; + + constexpr float ubl_3_point_1_X = UBL_PROBE_PT_1_X, + ubl_3_point_1_Y = UBL_PROBE_PT_1_Y, + ubl_3_point_2_X = UBL_PROBE_PT_2_X, + ubl_3_point_2_Y = UBL_PROBE_PT_2_Y, + ubl_3_point_3_X = UBL_PROBE_PT_3_X, + ubl_3_point_3_Y = UBL_PROBE_PT_3_Y; #define SIZE_OF_LITTLE_RAISE 0 #define BIG_RAISE_NOT_NEEDED 0 @@ -293,19 +296,16 @@ volatile uint8_t ubl_encoderDiff = 0; // Volatile because it's changed by Temperature ISR button update // The simple parameter flags and values are 'static' so parameter parsing can be in a support routine. - static int g29_verbose_level = 0, test_value = 0, - phase_value = -1, repetition_cnt = 1; + static int g29_verbose_level = 0, phase_value = -1, repetition_cnt = 1, + storage_slot = 0, test_pattern = 0; static bool repeat_flag = UBL_OK, c_flag = false, x_flag = UBL_OK, y_flag = UBL_OK, statistics_flag = UBL_OK, business_card_mode = false; static float x_pos = 0.0, y_pos = 0.0, height_value = 5.0, measured_z, card_thickness = 0.0, constant = 0.0; - static int storage_slot = 0, test_pattern = 0; #if ENABLED(ULTRA_LCD) void lcd_setstatus(const char* message, bool persist); #endif void gcode_G29() { - mesh_index_pair location; - int j, k; float Z1, Z2, Z3; g29_verbose_level = 0; // These may change, but let's get some reasonable values into them. @@ -331,7 +331,7 @@ if (code_seen('I')) { repetition_cnt = code_has_value() ? code_value_int() : 1; while (repetition_cnt--) { - location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position + const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position if (location.x_index < 0) { SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n"); break; // No more invalid Mesh Points to populate @@ -409,8 +409,8 @@ SERIAL_ECHOPAIR(",", y_pos); SERIAL_PROTOCOLLNPGM(")\n"); } - probe_entire_mesh( x_pos+X_PROBE_OFFSET_FROM_EXTRUDER, y_pos+Y_PROBE_OFFSET_FROM_EXTRUDER, - code_seen('O') || code_seen('M'), code_seen('E')); + probe_entire_mesh(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER, + code_seen('O') || code_seen('M'), code_seen('E')); break; // // Manually Probe Mesh in areas that can not be reached by the probe @@ -455,7 +455,7 @@ // If no repetition is specified, do the whole Mesh if (!repeat_flag) repetition_cnt = 9999; while (repetition_cnt--) { - location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position + const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position if (location.x_index < 0) break; // No more invalid Mesh Points to populate z_values[location.x_index][location.y_index] = height_value; } @@ -534,8 +534,7 @@ if (code_seen('L')) { // Load Current Mesh Data storage_slot = code_has_value() ? code_value_int() : ubl.state.eeprom_storage_slot; - k = E2END - sizeof(ubl.state); - j = (k - ubl_eeprom_start) / sizeof(z_values); + const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values); if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) { SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); @@ -569,8 +568,7 @@ return; } - int k = E2END - sizeof(ubl.state), - j = (k - ubl_eeprom_start) / sizeof(z_values); + const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values); if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) { SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); @@ -691,7 +689,7 @@ z_values[x][y] -= mean + constant; } - void shift_mesh_height( ) { + void shift_mesh_height() { for (uint8_t x = 0; x < UBL_MESH_NUM_X_POINTS; x++) for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++) if (!isnan(z_values[x][y])) @@ -702,9 +700,8 @@ * Probe all invalidated locations of the mesh that can be reached by the probe. * This attempts to fill in locations closest to the nozzle's start location first. */ - void probe_entire_mesh(float x_pos, float y_pos, bool do_ubl_mesh_map, bool stow_probe) { + void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe) { mesh_index_pair location; - float xProbe, yProbe, measured_z; ubl_has_control_of_lcd_panel++; save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe @@ -720,20 +717,22 @@ restore_ubl_active_state_and_leave(); return; } - location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 1, NULL); // the '1' says we want the location to be relative to the probe + + location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL); // the '1' says we want the location to be relative to the probe if (location.x_index >= 0 && location.y_index >= 0) { - xProbe = ubl.map_x_index_to_bed_location(location.x_index); - yProbe = ubl.map_y_index_to_bed_location(location.y_index); + const float xProbe = ubl.map_x_index_to_bed_location(location.x_index), + yProbe = ubl.map_y_index_to_bed_location(location.y_index); if (xProbe < MIN_PROBE_X || xProbe > MAX_PROBE_X || yProbe < MIN_PROBE_Y || yProbe > MAX_PROBE_Y) { SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed."); ubl_has_control_of_lcd_panel = false; goto LEAVE; } - measured_z = probe_pt(xProbe, yProbe, stow_probe, g29_verbose_level); + const float measured_z = probe_pt(xProbe, yProbe, stow_probe, g29_verbose_level); z_values[location.x_index][location.y_index] = measured_z + Z_PROBE_OFFSET_FROM_EXTRUDER; } if (do_ubl_mesh_map) ubl.display_map(1); + } while (location.x_index >= 0 && location.y_index >= 0); LEAVE: @@ -742,32 +741,27 @@ STOW_PROBE(); restore_ubl_active_state_and_leave(); - x_pos = constrain(x_pos - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS); - y_pos = constrain(y_pos - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS); - - do_blocking_move_to_xy(x_pos, y_pos); + do_blocking_move_to_xy( + constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS), + constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS) + ); } - vector tilt_mesh_based_on_3pts(float pt1, float pt2, float pt3) { - vector v1, v2, normal; + vector_3 tilt_mesh_based_on_3pts(const float &pt1, const float &pt2, const float &pt3) { float c, d, t; int i, j; - v1.dx = (ubl_3_point_1_X - ubl_3_point_2_X); - v1.dy = (ubl_3_point_1_Y - ubl_3_point_2_Y); - v1.dz = (pt1 - pt2); + vector_3 v1 = vector_3( (ubl_3_point_1_X - ubl_3_point_2_X), + (ubl_3_point_1_Y - ubl_3_point_2_Y), + (pt1 - pt2) ), - v2.dx = (ubl_3_point_3_X - ubl_3_point_2_X); - v2.dy = (ubl_3_point_3_Y - ubl_3_point_2_Y); - v2.dz = (pt3 - pt2); + v2 = vector_3( (ubl_3_point_3_X - ubl_3_point_2_X), + (ubl_3_point_3_Y - ubl_3_point_2_Y), + (pt3 - pt2) ), - // do cross product + normal = vector_3::cross(v1, v2); - normal.dx = v1.dy * v2.dz - v1.dz * v2.dy; - normal.dy = v1.dz * v2.dx - v1.dx * v2.dz; - normal.dz = v1.dx * v2.dy - v1.dy * v2.dx; - - // printf("[%f,%f,%f] ", normal.dx, normal.dy, normal.dz); + // printf("[%f,%f,%f] ", normal.x, normal.y, normal.z); /** * This code does two things. This vector is normal to the tilted plane. @@ -776,31 +770,32 @@ * We also need Z to be unity because we are going to be treating this triangle * as the sin() and cos() of the bed's tilt */ - normal.dx /= normal.dz; - normal.dy /= normal.dz; - normal.dz /= normal.dz; + const float inv_z = 1.0 / normal.z; + normal.x *= inv_z; + normal.y *= inv_z; + normal.z = 1.0; // // All of 3 of these points should give us the same d constant // - t = normal.dx * ubl_3_point_1_X + normal.dy * ubl_3_point_1_Y; - d = t + normal.dz * pt1; + t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y; + d = t + normal.z * pt1; c = d - t; SERIAL_ECHOPGM("d from 1st point: "); SERIAL_ECHO_F(d, 6); SERIAL_ECHOPGM(" c: "); SERIAL_ECHO_F(c, 6); SERIAL_EOL; - t = normal.dx * ubl_3_point_2_X + normal.dy * ubl_3_point_2_Y; - d = t + normal.dz * pt2; + t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y; + d = t + normal.z * pt2; c = d - t; SERIAL_ECHOPGM("d from 2nd point: "); SERIAL_ECHO_F(d, 6); SERIAL_ECHOPGM(" c: "); SERIAL_ECHO_F(c, 6); SERIAL_EOL; - t = normal.dx * ubl_3_point_3_X + normal.dy * ubl_3_point_3_Y; - d = t + normal.dz * pt3; + t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y; + d = t + normal.z * pt3; c = d - t; SERIAL_ECHOPGM("d from 3rd point: "); SERIAL_ECHO_F(d, 6); @@ -810,7 +805,7 @@ for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { - c = -((normal.dx * (UBL_MESH_MIN_X + i * (MESH_X_DIST)) + normal.dy * (UBL_MESH_MIN_Y + j * (MESH_Y_DIST))) - d); + c = -((normal.x * (UBL_MESH_MIN_X + i * (MESH_X_DIST)) + normal.y * (UBL_MESH_MIN_Y + j * (MESH_Y_DIST))) - d); z_values[i][j] += c; } } @@ -829,7 +824,7 @@ return current_position[Z_AXIS]; } - float measure_business_card_thickness(float height_value) { + float measure_business_card_thickness(const float &height_value) { ubl_has_control_of_lcd_panel++; save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe @@ -856,44 +851,45 @@ return abs(Z1 - Z2); } - void manually_probe_remaining_mesh(float x_pos, float y_pos, float z_clearance, float card_thickness, bool do_ubl_mesh_map) { - mesh_index_pair location; - float last_x, last_y, dx, dy, - xProbe, yProbe; + void manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &card_thickness, const bool do_ubl_mesh_map) { ubl_has_control_of_lcd_panel++; - last_x = last_y = -9999.99; save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe do_blocking_move_to_z(z_clearance); - do_blocking_move_to_xy(x_pos, y_pos); + do_blocking_move_to_xy(lx, ly); + float last_x = -9999.99, last_y = -9999.99; + mesh_index_pair location; do { if (do_ubl_mesh_map) ubl.display_map(1); - location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, 0, NULL); // The '0' says we want to use the nozzle's position + location = find_closest_mesh_point_of_type(INVALID, lx, ly, 0, NULL); // The '0' says we want to use the nozzle's position // It doesn't matter if the probe can not reach the // NAN location. This is a manual probe. if (location.x_index < 0 && location.y_index < 0) continue; - xProbe = ubl.map_x_index_to_bed_location(location.x_index); - yProbe = ubl.map_y_index_to_bed_location(location.y_index); + const float xProbe = ubl.map_x_index_to_bed_location(location.x_index), + yProbe = ubl.map_y_index_to_bed_location(location.y_index); + + // Modify to use if (position_is_reachable(pos[XYZ])) if (xProbe < (X_MIN_POS) || xProbe > (X_MAX_POS) || yProbe < (Y_MIN_POS) || yProbe > (Y_MAX_POS)) { SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed."); ubl_has_control_of_lcd_panel = false; goto LEAVE; } - dx = xProbe - last_x; - dy = yProbe - last_y; + const float dx = xProbe - last_x, + dy = yProbe - last_y; if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED) do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); else do_blocking_move_to_z(z_clearance); + do_blocking_move_to_xy(xProbe, yProbe); + last_x = xProbe; last_y = yProbe; - do_blocking_move_to_xy(xProbe, yProbe); wait_for_user = true; while (wait_for_user) { // we need the loop to move the nozzle based on the encoder wheel here! @@ -931,7 +927,7 @@ LEAVE: restore_ubl_active_state_and_leave(); do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); - do_blocking_move_to_xy(x_pos, y_pos); + do_blocking_move_to_xy(lx, ly); } bool g29_parameter_parsing() { @@ -983,7 +979,7 @@ ubl.store_state(); } - if ((c_flag = code_seen('C')) && code_has_value()) + if ((c_flag = code_seen('C') && code_has_value())) constant = code_value_float(); if (code_seen('D')) { // Disable the Unified Bed Leveling System @@ -992,19 +988,17 @@ ubl.store_state(); } - if (code_seen('F')) { - ubl.state.g29_correction_fade_height = 10.00; - if (code_has_value()) { - ubl.state.g29_correction_fade_height = code_value_float(); - ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height; + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + if (code_seen('F') && code_has_value()) { + const float fh = code_value_float(); + if (fh < 0.0 || fh > 100.0) { + SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n"); + return UBL_ERR; + } + ubl.state.g29_correction_fade_height = fh; + ubl.state.g29_fade_height_multiplier = 1.0 / fh; } - if (ubl.state.g29_correction_fade_height < 0.0 || ubl.state.g29_correction_fade_height > 100.0) { - SERIAL_PROTOCOLLNPGM("?Bed Level Correction Fade Height Not Plausible.\n"); - ubl.state.g29_correction_fade_height = 10.00; - ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height; - return UBL_ERR; - } - } + #endif if ((repeat_flag = code_seen('R'))) { repetition_cnt = code_has_value() ? code_value_int() : 9999; @@ -1020,7 +1014,7 @@ * This function goes away after G29 debug is complete. But for right now, it is a handy * routine to dump binary data structures. */ - void dump(char *str, float f) { + void dump(char * const str, const float &f) { char *ptr; SERIAL_PROTOCOL(str); @@ -1056,7 +1050,6 @@ } ubl_state_at_invocation = ubl.state.active; ubl.state.active = 0; - return; } void restore_ubl_active_state_and_leave() { @@ -1075,33 +1068,27 @@ * good to have the extra information. Soon... we prune this to just a few items */ void g29_what_command() { - int k = E2END - ubl_eeprom_start; + const uint16_t k = E2END - ubl_eeprom_start; statistics_flag++; SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version 1.00 "); - if (ubl.state.active) - SERIAL_PROTOCOLPGM("Active.\n"); - else - SERIAL_PROTOCOLPGM("Inactive.\n"); - SERIAL_EOL; + ubl.state.active ? SERIAL_PROTOCOLCHAR('A') : SERIAL_PROTOCOLPGM("In"); + SERIAL_PROTOCOLLNPGM("ctive.\n"); delay(50); - if (ubl.state.eeprom_storage_slot == 0xFFFF) { + if (ubl.state.eeprom_storage_slot == -1) SERIAL_PROTOCOLPGM("No Mesh Loaded."); - } else { SERIAL_PROTOCOLPGM("Mesh: "); prt_hex_word(ubl.state.eeprom_storage_slot); - SERIAL_PROTOCOLPGM(" Loaded. "); + SERIAL_PROTOCOLPGM(" Loaded."); } - - SERIAL_EOL; - delay(50); - - SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height ); SERIAL_EOL; - idle(); + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height); + SERIAL_EOL; + #endif SERIAL_PROTOCOLPGM("z_offset: "); SERIAL_PROTOCOL_F(ubl.state.z_offset, 6); @@ -1111,28 +1098,20 @@ for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { SERIAL_PROTOCOL_F( ubl.map_x_index_to_bed_location(i), 1); SERIAL_PROTOCOLPGM(" "); - delay(10); } SERIAL_EOL; - delay(50); - idle(); SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); for (uint8_t i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) { SERIAL_PROTOCOL_F( ubl.map_y_index_to_bed_location(i), 1); SERIAL_PROTOCOLPGM(" "); - delay(10); } SERIAL_EOL; - delay(50); - idle(); #if HAS_KILL SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN); SERIAL_PROTOCOLLNPAIR(" state:", READ(KILL_PIN)); #endif - delay(50); - idle(); SERIAL_EOL; SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation); @@ -1142,54 +1121,39 @@ SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x"); prt_hex_word(ubl_eeprom_start); SERIAL_EOL; - delay(50); - idle(); SERIAL_PROTOCOLPGM("end of EEPROM : "); prt_hex_word(E2END); SERIAL_EOL; - delay(50); - idle(); SERIAL_PROTOCOLLNPAIR("sizeof(ubl) : ", (int)sizeof(ubl)); SERIAL_EOL; SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values)); SERIAL_EOL; - delay(50); - idle(); SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x"); prt_hex_word(k); SERIAL_EOL; - idle(); SERIAL_PROTOCOLPGM("EEPROM can hold 0x"); prt_hex_word(k / sizeof(z_values)); SERIAL_PROTOCOLLNPGM(" meshes.\n"); - delay(50); SERIAL_PROTOCOLPGM("sizeof(ubl.state) :"); prt_hex_word(sizeof(ubl.state)); - idle(); SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_X_POINTS ", UBL_MESH_NUM_X_POINTS); SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MIN_X ", UBL_MESH_MIN_X); - delay(50); - idle(); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MIN_Y ", UBL_MESH_MIN_Y); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y); - delay(50); - idle(); SERIAL_PROTOCOLPGM("\nMESH_X_DIST "); SERIAL_PROTOCOL_F(MESH_X_DIST, 6); SERIAL_PROTOCOLPGM("\nMESH_Y_DIST "); SERIAL_PROTOCOL_F(MESH_Y_DIST, 6); SERIAL_EOL; - idle(); - if (!ubl.sanity_check()) SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed."); } @@ -1205,7 +1169,7 @@ SERIAL_ECHO_START; SERIAL_ECHOLNPGM("EEPROM Dump:"); for (uint16_t i = 0; i < E2END + 1; i += 16) { - if (i & 0x3 == 0) idle(); + if (!(i & 0x3)) idle(); prt_hex_word(i); SERIAL_ECHOPGM(": "); for (uint16_t j = 0; j < 16; j++) { @@ -1217,7 +1181,6 @@ SERIAL_EOL; } SERIAL_EOL; - return; } /** @@ -1233,15 +1196,14 @@ } storage_slot = code_value_int(); - uint16_t k = E2END - sizeof(ubl.state), - j = (k - ubl_eeprom_start) / sizeof(tmp_z_values); + int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(tmp_z_values); if (storage_slot < 0 || storage_slot > j || ubl_eeprom_start <= 0) { SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); return; } - j = k - (storage_slot + 1) * sizeof(tmp_z_values); + j = UBL_LAST_EEPROM_INDEX - (storage_slot + 1) * sizeof(tmp_z_values); eeprom_read_block((void *)&tmp_z_values, (void *)j, sizeof(tmp_z_values)); SERIAL_ECHOPAIR("Subtracting Mesh ", storage_slot); @@ -1254,23 +1216,19 @@ z_values[x][y] = z_values[x][y] - tmp_z_values[x][y]; } - mesh_index_pair find_closest_mesh_point_of_type(MeshPointType type, float X, float Y, bool probe_as_reference, unsigned int bits[16]) { + 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]) { int i, j; - float f, px, py, mx, my, dx, dy, closest = 99999.99, - current_x, current_y, distance; + float closest = 99999.99; mesh_index_pair return_val; return_val.x_index = return_val.y_index = -1; - current_x = current_position[X_AXIS]; - current_y = current_position[Y_AXIS]; + const float current_x = current_position[X_AXIS], + current_y = current_position[Y_AXIS]; - px = X; // Get our reference position. Either the nozzle or - py = Y; // the probe location. - if (probe_as_reference) { - px -= X_PROBE_OFFSET_FROM_EXTRUDER; - py -= Y_PROBE_OFFSET_FROM_EXTRUDER; - } + // Get our reference position. Either the nozzle or probe location. + const float px = lx - (probe_as_reference ? X_PROBE_OFFSET_FROM_EXTRUDER : 0), + py = ly - (probe_as_reference ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0); for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { @@ -1282,24 +1240,20 @@ // We only get here if we found a Mesh Point of the specified type - mx = ubl.map_x_index_to_bed_location(i); // Check if we can probe this mesh location - my = ubl.map_y_index_to_bed_location(j); + const float mx = LOGICAL_X_POSITION(ubl.map_x_index_to_bed_location(i)), // Check if we can probe this mesh location + my = LOGICAL_Y_POSITION(ubl.map_y_index_to_bed_location(j)); // If we are using the probe as the reference there are some locations we can't get to. // We prune these out of the list and ignore them until the next Phase where we do the // manual nozzle probing. - + if (probe_as_reference && - ( mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X) || my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y) ) + (mx < (MIN_PROBE_X) || mx > (MAX_PROBE_X) || my < (MIN_PROBE_Y) || my > (MAX_PROBE_Y)) ) continue; - dx = px - mx; // We can get to it. Let's see if it is the - dy = py - my; // closest location to the nozzle. - distance = HYPOT(dx, dy); - - dx = current_x - mx; // We are going to add in a weighting factor that considers - dy = current_y - my; // the current location of the nozzle. If two locations are equal - distance += HYPOT(dx, dy) * 0.01; // distance from the measurement location, we are going to give + // We can get to it. Let's see if it is the closest location to the nozzle. + // Add in a weighting factor that considers the current location of the nozzle. + const float distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.01; if (distance < closest) { closest = distance; // We found a closer location with @@ -1313,10 +1267,9 @@ return return_val; } - void fine_tune_mesh(float x_pos, float y_pos, bool do_ubl_mesh_map) { + void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) { mesh_index_pair location; - float xProbe, yProbe; - uint16_t i, not_done[16]; + uint16_t not_done[16]; int32_t round_off; save_ubl_active_state_and_disable(); @@ -1327,11 +1280,11 @@ #endif do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); - do_blocking_move_to_xy(x_pos, y_pos); + do_blocking_move_to_xy(lx, ly); do { if (do_ubl_mesh_map) ubl.display_map(1); - location = find_closest_mesh_point_of_type( SET_IN_BITMAP, x_pos, y_pos, 0, not_done); // The '0' says we want to use the nozzle's position + location = find_closest_mesh_point_of_type( SET_IN_BITMAP, lx, ly, 0, not_done); // The '0' says we want to use the nozzle's position // It doesn't matter if the probe can not reach this // location. This is a manual edit of the Mesh Point. if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points. @@ -1339,8 +1292,8 @@ 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 - xProbe = ubl.map_x_index_to_bed_location(location.x_index); - yProbe = ubl.map_y_index_to_bed_location(location.y_index); + const float xProbe = ubl.map_x_index_to_bed_location(location.x_index), + yProbe = ubl.map_y_index_to_bed_location(location.y_index); if (xProbe < X_MIN_POS || xProbe > X_MAX_POS || yProbe < Y_MIN_POS || yProbe > Y_MAX_POS) { // In theory, we don't need this check. SERIAL_PROTOCOLLNPGM("?Error: Attempt to edit off the bed."); // This really can't happen, but for now, ubl_has_control_of_lcd_panel = false; // Let's do the check. @@ -1406,7 +1359,7 @@ restore_ubl_active_state_and_leave(); do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); - do_blocking_move_to_xy(x_pos, y_pos); + do_blocking_move_to_xy(lx, ly); #if ENABLED(ULTRA_LCD) lcd_setstatus("Done Editing Mesh", true); diff --git a/Marlin/UBL_line_to_destination.cpp b/Marlin/UBL_line_to_destination.cpp index 2b3b1988f9..91bf3e3519 100644 --- a/Marlin/UBL_line_to_destination.cpp +++ b/Marlin/UBL_line_to_destination.cpp @@ -29,35 +29,97 @@ #include #include + extern float destination[XYZE]; extern void set_current_to_destination(); - extern void debug_current_and_destination(char *title); + + void debug_current_and_destination(char *title) { + + // if the title message starts with a '!' it is so important, we are going to + // ignore the status of the g26_debug_flag + if (*title != '!' && !g26_debug_flag) return; + + const float de = destination[E_AXIS] - current_position[E_AXIS]; + + if (de == 0.0) return; + + const float dx = current_position[X_AXIS] - destination[X_AXIS], + dy = current_position[Y_AXIS] - destination[Y_AXIS], + xy_dist = HYPOT(dx, dy); + + if (xy_dist == 0.0) { + return; + //SERIAL_ECHOPGM(" FPMM="); + //const float fpmm = de / xy_dist; + //SERIAL_PROTOCOL_F(fpmm, 6); + } + else { + SERIAL_ECHOPGM(" fpmm="); + const float fpmm = de / xy_dist; + SERIAL_ECHO_F(fpmm, 6); + } + + SERIAL_ECHOPGM(" current=( "); + SERIAL_ECHO_F(current_position[X_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[Y_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[Z_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[E_AXIS], 6); + SERIAL_ECHOPGM(" ) destination=( "); + if (current_position[X_AXIS] == destination[X_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_ECHO_F(destination[X_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[Y_AXIS] == destination[Y_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_ECHO_F(destination[Y_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[Z_AXIS] == destination[Z_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_ECHO_F(destination[Z_AXIS], 6); + + SERIAL_ECHOPGM(", "); + + if (current_position[E_AXIS] == destination[E_AXIS]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_ECHO_F(destination[E_AXIS], 6); + + SERIAL_ECHOPGM(" ) "); + SERIAL_ECHO(title); + SERIAL_EOL; + + SET_INPUT_PULLUP(66); // Roxy's Left Switch is on pin 66. Right Switch is on pin 65 + + //if (been_to_2_6) { + //while ((digitalRead(66) & 0x01) != 0) + // idle(); + //} + } void ubl_line_to_destination(const float &x_end, const float &y_end, const float &z_end, const float &e_end, const float &feed_rate, uint8_t extruder) { - - int cell_start_xi, cell_start_yi, cell_dest_xi, cell_dest_yi, - current_xi, current_yi, - dxi, dyi, xi_cnt, yi_cnt; - float x_start, y_start, - x, y, z1, z2, z0 /*, z_optimized */, - next_mesh_line_x, next_mesh_line_y, a0ma1diva2ma1, - on_axis_distance, e_normalized_dist, e_position, e_start, z_normalized_dist, z_position, z_start, - dx, dy, adx, ady, m, c; - /** * 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 * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave */ + const float x_start = current_position[X_AXIS], + y_start = current_position[Y_AXIS], + z_start = current_position[Z_AXIS], + e_start = current_position[E_AXIS]; - x_start = current_position[X_AXIS]; - y_start = current_position[Y_AXIS]; - z_start = current_position[Z_AXIS]; - e_start = current_position[E_AXIS]; - - cell_start_xi = ubl.get_cell_index_x(x_start); - cell_start_yi = ubl.get_cell_index_y(y_start); - cell_dest_xi = ubl.get_cell_index_x(x_end); - cell_dest_yi = ubl.get_cell_index_y(y_end); + const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_start)), + cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_start)), + cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_end)), + cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_end)); if (g26_debug_flag) { SERIAL_ECHOPGM(" ubl_line_to_destination(xe="); @@ -68,7 +130,7 @@ SERIAL_ECHO(z_end); SERIAL_ECHOPGM(", ee="); SERIAL_ECHO(e_end); - SERIAL_ECHOPGM(")\n"); + SERIAL_ECHOLNPGM(")"); debug_current_and_destination((char*)"Start of ubl_line_to_destination()"); } @@ -82,7 +144,7 @@ if (cell_dest_xi < 0 || cell_dest_yi < 0 || cell_dest_xi >= UBL_MESH_NUM_X_POINTS || cell_dest_yi >= UBL_MESH_NUM_Y_POINTS) { - // 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. planner.buffer_line(x_end, y_end, z_end + ubl.state.z_offset, e_end, feed_rate, extruder); @@ -105,20 +167,18 @@ * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. */ - a0ma1diva2ma1 = (x_end - mesh_index_to_x_location[cell_dest_xi]) * 0.1 * (MESH_X_DIST); - - z1 = z_values[cell_dest_xi ][cell_dest_yi ] + a0ma1diva2ma1 * - (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]); - - z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + a0ma1diva2ma1 * - (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]); + const float xratio = (RAW_X_POSITION(x_end) - mesh_index_to_x_location[cell_dest_xi]) * (1.0 / (MESH_X_DIST)), + z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio * + (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]), + z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio * + (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 // are going to apply the Y-Distance into the cell to interpolate the final Z correction. - a0ma1diva2ma1 = (y_end - mesh_index_to_y_location[cell_dest_yi]) * 0.1 * (MESH_Y_DIST); + const float yratio = (RAW_Y_POSITION(y_end) - mesh_index_to_y_location[cell_dest_yi]) * (1.0 / (MESH_Y_DIST)); - z0 = z1 + (z2 - z1) * a0ma1diva2ma1; + float z0 = z1 + (z2 - z1) * yratio; /** * Debug code to use non-optimized get_z_correction() and to do a sanity check @@ -126,7 +186,7 @@ */ /* z_optimized = z0; - z0 = ubl.get_z_correction( x_end, y_end); + z0 = ubl.get_z_correction(x_end, y_end); if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) { debug_current_and_destination((char*)"FINAL_MOVE: z_correction()"); if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); @@ -139,7 +199,7 @@ SERIAL_EOL; } //*/ - z0 = z0 * ubl.fade_scaling_factor_for_z(z_end); + z0 *= ubl.fade_scaling_factor_for_z(z_end); /** * If part of the Mesh is undefined, it will show up as NAN @@ -167,31 +227,17 @@ * blocks of code: */ - dx = x_end - x_start; - dy = y_end - y_start; + const float dx = x_end - x_start, + dy = y_end - y_start; const int left_flag = dx < 0.0 ? 1 : 0, down_flag = dy < 0.0 ? 1 : 0; - if (left_flag) { // figure out which way we need to move to get to the next cell - dxi = -1; - adx = -dx; // absolute value of dx. We already need to check if dx and dy are negative. - } - else { // We may as well generate the appropriate values for adx and ady right now - dxi = 1; // to save setting up the abs() function call and actually doing the call. - adx = dx; - } - if (dy < 0.0) { - dyi = -1; - ady = -dy; // absolute value of dy - } - else { - dyi = 1; - ady = dy; - } + const float adx = left_flag ? -dx : dx, + ady = down_flag ? -dy : dy; - if (cell_start_xi == cell_dest_xi) dxi = 0; - if (cell_start_yi == cell_dest_yi) dyi = 0; + const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1, + dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1; /** * Compute the scaling factor for the extruder for each partial move. @@ -204,22 +250,20 @@ const bool use_x_dist = adx > ady; - on_axis_distance = use_x_dist ? x_end - x_start : y_end - y_start; + float on_axis_distance = use_x_dist ? dx : dy, + e_position = e_end - e_start, + z_position = z_end - z_start; - e_position = e_end - e_start; - e_normalized_dist = e_position / on_axis_distance; + const float e_normalized_dist = e_position / on_axis_distance, + z_normalized_dist = z_position / on_axis_distance; - z_position = z_end - z_start; - z_normalized_dist = z_position / on_axis_distance; + int current_xi = cell_start_xi, current_yi = cell_start_yi; - const bool inf_normalized_flag = e_normalized_dist == INFINITY || e_normalized_dist == -INFINITY; + const float m = dy / dx, + c = y_start - m * x_start; - current_xi = cell_start_xi; - current_yi = cell_start_yi; - - m = dy / dx; - c = y_start - m * x_start; - const bool inf_m_flag = (m == INFINITY || m == -INFINITY); + const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance), + inf_m_flag = NEAR_ZERO(dx); /** * This block handles vertical lines. These are lines that stay within the same @@ -230,16 +274,16 @@ current_yi += down_flag; // Line is heading down, we just want to go to the bottom while (current_yi != cell_dest_yi + down_flag) { current_yi += dyi; - next_mesh_line_y = mesh_index_to_y_location[current_yi]; + const float next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]); /** * inf_m_flag? the slope of the line is infinite, we won't do the calculations * else, we know the next X is the same so we can recover and continue! * Calculate X at the next Y mesh line */ - x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m; + const float x = inf_m_flag ? x_start : (next_mesh_line_y - c) / m; - z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi); + float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi, current_yi); /** * Debug code to use non-optimized get_z_correction() and to do a sanity check @@ -247,7 +291,7 @@ */ /* z_optimized = z0; - z0 = ubl.get_z_correction( x, next_mesh_line_y); + z0 = ubl.get_z_correction(x, next_mesh_line_y); if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) { debug_current_and_destination((char*)"VERTICAL z_correction()"); if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); @@ -261,7 +305,7 @@ } //*/ - z0 = z0 * ubl.fade_scaling_factor_for_z(z_end); + z0 *= ubl.fade_scaling_factor_for_z(z_end); /** * If part of the Mesh is undefined, it will show up as NAN @@ -272,7 +316,7 @@ */ if (isnan(z0)) z0 = 0.0; - y = mesh_index_to_y_location[current_yi]; + const float y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi]); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -321,10 +365,10 @@ // edge of this cell for the first move. while (current_xi != cell_dest_xi + left_flag) { current_xi += dxi; - next_mesh_line_x = mesh_index_to_x_location[current_xi]; - y = m * next_mesh_line_x + c; // Calculate X at the next Y mesh line + const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]), + y = m * next_mesh_line_x + c; // Calculate X at the next Y mesh line - z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi); + float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi, current_yi); /** * Debug code to use non-optimized get_z_correction() and to do a sanity check @@ -332,7 +376,7 @@ */ /* z_optimized = z0; - z0 = ubl.get_z_correction( next_mesh_line_x, y); + z0 = ubl.get_z_correction(next_mesh_line_x, y); if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) { debug_current_and_destination((char*)"HORIZONTAL z_correction()"); if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); @@ -357,7 +401,7 @@ */ if (isnan(z0)) z0 = 0.0; - x = mesh_index_to_x_location[current_xi]; + const float x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi]); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -396,10 +440,10 @@ * */ - xi_cnt = cell_start_xi - cell_dest_xi; - if (xi_cnt < 0) xi_cnt = -xi_cnt; + int xi_cnt = cell_start_xi - cell_dest_xi, + yi_cnt = cell_start_yi - cell_dest_yi; - yi_cnt = cell_start_yi - cell_dest_yi; + if (xi_cnt < 0) xi_cnt = -xi_cnt; if (yi_cnt < 0) yi_cnt = -yi_cnt; current_xi += left_flag; @@ -407,20 +451,19 @@ while (xi_cnt > 0 || yi_cnt > 0) { - next_mesh_line_x = mesh_index_to_x_location[current_xi + dxi]; - next_mesh_line_y = mesh_index_to_y_location[current_yi + dyi]; - - y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line - x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line (we don't have to worry - // about m being equal to 0.0 If this was the case, we would have - // detected this as a vertical line move up above and we wouldn't - // be down here doing a generic type of move. + const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi + dxi]), + next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_y_location[current_yi + dyi]), + y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line + x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line (we don't have to worry + // about m being equal to 0.0 If this was the case, we would have + // detected this as a vertical line move up above and we wouldn't + // be down here doing a generic type of move. if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first // // Yes! Crossing a Y Mesh Line next // - z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi); + float z0 = ubl.get_z_correction_along_horizontal_mesh_line_at_specific_X(x, current_xi - left_flag, current_yi + dyi); /** * Debug code to use non-optimized get_z_correction() and to do a sanity check @@ -428,7 +471,7 @@ */ /* z_optimized = z0; - z0 = ubl.get_z_correction( x, next_mesh_line_y); + z0 = ubl.get_z_correction(x, next_mesh_line_y); if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) { debug_current_and_destination((char*)"General_1: z_correction()"); if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); @@ -471,7 +514,7 @@ // // Yes! Crossing a X Mesh Line next // - z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag); + float z0 = ubl.get_z_correction_along_vertical_mesh_line_at_specific_Y(y, current_xi + dxi, current_yi - down_flag); /** * Debug code to use non-optimized get_z_correction() and to do a sanity check @@ -479,7 +522,7 @@ */ /* z_optimized = z0; - z0 = ubl.get_z_correction( next_mesh_line_x, y); + z0 = ubl.get_z_correction(next_mesh_line_x, y); if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) { debug_current_and_destination((char*)"General_2: z_correction()"); if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); @@ -493,7 +536,7 @@ } //*/ - z0 = z0 * ubl.fade_scaling_factor_for_z(z_end); + z0 *= ubl.fade_scaling_factor_for_z(z_end); /** * If part of the Mesh is undefined, it will show up as NAN diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp index 80c9976488..9586e6cd7b 100644 --- a/Marlin/configuration_store.cpp +++ b/Marlin/configuration_store.cpp @@ -847,8 +847,8 @@ void Config_Postprocess() { #if ENABLED(AUTO_BED_LEVELING_UBL) ubl_eeprom_start = (eeprom_index + 32) & 0xFFF8; // Pad the end of configuration data so it - // can float up or down a little bit without - // disrupting the Unified Bed Leveling data + // can float up or down a little bit without + // disrupting the Unified Bed Leveling data ubl.load_state(); SERIAL_ECHOPGM(" UBL "); @@ -879,7 +879,7 @@ void Config_Postprocess() { } else { ubl.reset(); - SERIAL_ECHOPGM("UBL System reset() \n"); + SERIAL_ECHOLNPGM("UBL System reset()"); } #endif } @@ -1178,42 +1178,6 @@ void Config_ResetDefault() { SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS]); SERIAL_EOL; #endif - #if ENABLED(AUTO_BED_LEVELING_UBL) - SERIAL_ECHOLNPGM("Unified Bed Leveling:"); - CONFIG_ECHO_START; - - SERIAL_ECHOPGM("System is: "); - if (ubl.state.active) - SERIAL_ECHOLNPGM("Active\n"); - else - SERIAL_ECHOLNPGM("Deactive\n"); - - SERIAL_ECHOPAIR("Active Mesh Slot: ", ubl.state.eeprom_storage_slot); - SERIAL_EOL; - - SERIAL_ECHOPGM("z_offset: "); - SERIAL_ECHO_F(ubl.state.z_offset, 6); - SERIAL_EOL; - - SERIAL_ECHOPAIR("EEPROM can hold ", (int)((E2END - sizeof(ubl.state) - ubl_eeprom_start) / sizeof(z_values))); - SERIAL_ECHOLNPGM(" meshes. \n"); - - SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS ", UBL_MESH_NUM_X_POINTS); - SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS); - - SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X ", UBL_MESH_MIN_X); - SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y ", UBL_MESH_MIN_Y); - - SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X); - SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y); - - SERIAL_ECHOPGM("\nMESH_X_DIST "); - SERIAL_ECHO_F(MESH_X_DIST, 6); - SERIAL_ECHOPGM("\nMESH_Y_DIST "); - SERIAL_ECHO_F(MESH_Y_DIST, 6); - SERIAL_EOL; - SERIAL_EOL; - #endif #if HOTENDS > 1 CONFIG_ECHO_START; @@ -1233,6 +1197,7 @@ void Config_ResetDefault() { #endif #if ENABLED(MESH_BED_LEVELING) + if (!forReplay) { SERIAL_ECHOLNPGM("Mesh Bed Leveling:"); CONFIG_ECHO_START; @@ -1248,12 +1213,53 @@ void Config_ResetDefault() { SERIAL_EOL; } } + + #elif ENABLED(AUTO_BED_LEVELING_UBL) + + if (!forReplay) { + SERIAL_ECHOLNPGM("Unified Bed Leveling:"); + CONFIG_ECHO_START; + } + + SERIAL_ECHOLNPAIR(" M420 S", ubl.state.active ? 1 : 0); + + if (!forReplay) { + SERIAL_ECHOPGM("\nUBL is "); + ubl.state.active ? SERIAL_CHAR('A') : SERIAL_ECHOPGM("Ina"); + SERIAL_ECHOLNPAIR("ctive\n\nActive Mesh Slot: ", ubl.state.eeprom_storage_slot); + + SERIAL_ECHOPGM("z_offset: "); + SERIAL_ECHO_F(ubl.state.z_offset, 6); + SERIAL_EOL; + + SERIAL_ECHOPAIR("EEPROM can hold ", (int)((UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values))); + SERIAL_ECHOLNPGM(" meshes.\n"); + + SERIAL_ECHOPAIR("\nUBL_MESH_NUM_X_POINTS ", UBL_MESH_NUM_X_POINTS); + SERIAL_ECHOPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS); + + SERIAL_ECHOPAIR("\nUBL_MESH_MIN_X ", UBL_MESH_MIN_X); + SERIAL_ECHOPAIR("\nUBL_MESH_MIN_Y ", UBL_MESH_MIN_Y); + + SERIAL_ECHOPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X); + SERIAL_ECHOPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y); + + SERIAL_ECHOPGM("\nMESH_X_DIST "); + SERIAL_ECHO_F(MESH_X_DIST, 6); + SERIAL_ECHOPGM("\nMESH_Y_DIST "); + SERIAL_ECHO_F(MESH_Y_DIST, 6); + SERIAL_EOL; + SERIAL_EOL; + } + #elif HAS_ABL + if (!forReplay) { SERIAL_ECHOLNPGM("Auto Bed Leveling:"); CONFIG_ECHO_START; } SERIAL_ECHOLNPAIR(" M420 S", planner.abl_enabled ? 1 : 0); + #endif #if ENABLED(DELTA) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 1e6cd7bd05..62ddf0999a 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -530,7 +530,7 @@ void Planner::check_axes_activity() { #endif } -#if PLANNER_LEVELING +#if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) /** * lx, ly, lz - logical (cartesian, not delta) positions in mm */ @@ -634,7 +634,7 @@ void Planner::check_axes_activity() { #endif } -#endif // PLANNER_LEVELING +#endif // PLANNER_LEVELING && !AUTO_BED_LEVELING_UBL /** * Planner::_buffer_line @@ -1408,7 +1408,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c } void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) { - #if PLANNER_LEVELING + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] }; apply_leveling(lpos); #else diff --git a/Marlin/planner.h b/Marlin/planner.h index 1f16f6a7a7..38513baed4 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -244,7 +244,7 @@ class Planner { static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); } - #if PLANNER_LEVELING + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) #define ARG_X float lx #define ARG_Y float ly @@ -300,7 +300,7 @@ class Planner { * extruder - target extruder */ static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) { - #if PLANNER_LEVELING && IS_CARTESIAN + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) && IS_CARTESIAN apply_leveling(lx, ly, lz); #endif _buffer_line(lx, ly, lz, e, fr_mm_s, extruder); @@ -316,7 +316,7 @@ class Planner { * extruder - target extruder */ static FORCE_INLINE void buffer_line_kinematic(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) { - #if PLANNER_LEVELING + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) float lpos[XYZ] = { ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS] }; apply_leveling(lpos); #else @@ -340,7 +340,7 @@ class Planner { * Clears previous speed values. */ static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { - #if PLANNER_LEVELING && IS_CARTESIAN + #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL) && IS_CARTESIAN apply_leveling(lx, ly, lz); #endif _set_position_mm(lx, ly, lz, e);