CMS/Marlin
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Patches to bring UBL closer to compliance

Browse Source
This commit is contained in:
Scott Lahteine 2017-03-24 00:53:37 -05:00
parent d076c1b604
commit b5711a99a2
10 changed files with 493 additions and 543 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/Conditionals_post.h
View File

@ -669,7 +669,7 @@
#define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR)) #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 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)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#if HAS_PROBING_PROCEDURE #if HAS_PROBING_PROCEDURE

112
Marlin/G26_Mesh_Validation_Tool.cpp
View File

@ -24,15 +24,9 @@
* Marlin Firmware -- G26 - Mesh Validation Tool * Marlin Firmware -- G26 - Mesh Validation Tool
*/ */
#define EXTRUSION_MULTIPLIER 1.0 // This is too much clutter for the main Configuration.h file But #include "MarlinConfig.h"
#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 #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
#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 "Marlin.h" #include "Marlin.h"
#include "Configuration.h" #include "Configuration.h"
@ -42,7 +36,15 @@
#include "UBL.h" #include "UBL.h"
#include "ultralcd.h" #include "ultralcd.h"
#if ENABLED(AUTO_BED_LEVELING_UBL) #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_INTERSECTION_CIRCLES 5
#define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle. This number should be #define SIZE_OF_CROSS_HAIRS 3 // cross hairs inside the circle. This number should be
@ -156,7 +158,6 @@
float valid_trig_angle(float); float valid_trig_angle(float);
mesh_index_pair find_closest_circle_to_print(float, 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); 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 */ //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; int8_t prime_flag = 0;
bool keep_heaters_on = false; bool keep_heaters_on = false,
g26_debug_flag = 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); }
/** /**
* G26: Mesh Validation Pattern generation. * 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) { void 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;
@ -1002,4 +920,4 @@
return UBL_OK; return UBL_OK;
} }
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED

50
Marlin/Marlin_main.cpp
View File

@ -233,10 +233,6 @@
#include "duration_t.h" #include "duration_t.h"
#include "types.h" #include "types.h"
#if ENABLED(AUTO_BED_LEVELING_UBL)
#include "UBL.h"
#endif
#if HAS_ABL #if HAS_ABL
#include "vector_3.h" #include "vector_3.h"
#if ENABLED(AUTO_BED_LEVELING_LINEAR) #if ENABLED(AUTO_BED_LEVELING_LINEAR)
@ -301,7 +297,13 @@
#endif #endif
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#include "UBL.h"
unified_bed_leveling ubl; 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 #endif
bool Running = true; bool Running = true;
@ -2266,7 +2268,7 @@ static void clean_up_after_endstop_or_probe_move() {
#endif // HAS_BED_PROBE #endif // HAS_BED_PROBE
#if PLANNER_LEVELING || ENABLED(AUTO_BED_LEVELING_UBL) #if PLANNER_LEVELING
/** /**
* Turn bed leveling on or off, fixing the current * Turn bed leveling on or off, fixing the current
* position as-needed. * position as-needed.
@ -2310,6 +2312,7 @@ static void clean_up_after_endstop_or_probe_move() {
} }
#elif ENABLED(AUTO_BED_LEVELING_UBL) #elif ENABLED(AUTO_BED_LEVELING_UBL)
ubl.state.active = enable; ubl.state.active = enable;
//set_current_from_steppers_for_axis(Z_AXIS);
#endif #endif
} }
@ -3481,11 +3484,6 @@ inline void gcode_G4() {
* *
*/ */
inline void gcode_G28() { 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 ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
@ -3498,7 +3496,10 @@ inline void gcode_G28() {
stepper.synchronize(); stepper.synchronize();
// Disable the leveling matrix before homing // 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); set_bed_leveling_enabled(false);
#endif #endif
@ -5305,6 +5306,18 @@ inline void gcode_M42() {
#endif // Z_MIN_PROBE_REPEATABILITY_TEST #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 * M75: Start print timer
*/ */
@ -8512,7 +8525,7 @@ void process_next_command() {
break; break;
#endif // INCH_MODE_SUPPORT #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 case 26: // G26: Mesh Validation Pattern generation
gcode_G26(); gcode_G26();
break; break;
@ -8644,16 +8657,11 @@ void process_next_command() {
break; break;
#endif // Z_MIN_PROBE_REPEATABILITY_TEST #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 case 49: // M49: Turn on or off g26_debug_flag for verbose output
if (g26_debug_flag) { gcode_M49();
SERIAL_PROTOCOLPGM("UBL Debug Flag turned off.\n");
g26_debug_flag = 0; }
else {
SERIAL_PROTOCOLPGM("UBL Debug Flag turned on.\n");
g26_debug_flag++; }
break; break;
#endif // Z_MIN_PROBE_REPEATABILITY_TEST #endif // AUTO_BED_LEVELING_UBL && UBL_MESH_EDIT_ENABLED
case 75: // M75: Start print timer case 75: // M75: Start print timer
gcode_M75(); break; gcode_M75(); break;
@ -9547,7 +9555,7 @@ void get_cartesian_from_steppers() {
*/ */
void set_current_from_steppers_for_axis(const AxisEnum axis) { void set_current_from_steppers_for_axis(const AxisEnum axis) {
get_cartesian_from_steppers(); get_cartesian_from_steppers();
#if PLANNER_LEVELING #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
planner.unapply_leveling(cartes); planner.unapply_leveling(cartes);
#endif #endif
if (axis == ALL_AXES) if (axis == ALL_AXES)

132
Marlin/UBL.h
View File

@ -22,6 +22,7 @@
#include "Marlin.h" #include "Marlin.h"
#include "math.h" #include "math.h"
#include "vector_3.h"
#ifndef UNIFIED_BED_LEVELING_H #ifndef UNIFIED_BED_LEVELING_H
#define UNIFIED_BED_LEVELING_H #define UNIFIED_BED_LEVELING_H
@ -32,32 +33,29 @@
#define UBL_ERR true #define UBL_ERR true
typedef struct { typedef struct {
int x_index, y_index; int8_t x_index, y_index;
float distance; // Not always used. But when populated, it is the distance float distance; // When populated, the distance from the search location
// from the search location
} mesh_index_pair; } mesh_index_pair;
typedef struct { double dx, dy, dz; } vector;
enum MeshPointType { INVALID, REAL, SET_IN_BITMAP }; enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
bool axis_unhomed_error(bool, bool, bool); 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(); 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 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); void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
vector tilt_mesh_based_on_3pts(float, float, float); vector_3 tilt_mesh_based_on_3pts(const float&, const float&, const float&);
void new_set_bed_level_equation_3pts(float, float, float); float measure_business_card_thickness(const float&);
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]);
mesh_index_pair find_closest_mesh_point_of_type(MeshPointType, float, float, bool, unsigned int[16]);
void find_mean_mesh_height(); void find_mean_mesh_height();
void shift_mesh_height(); void shift_mesh_height();
bool g29_parameter_parsing(); bool g29_parameter_parsing();
void g29_what_command(); void g29_what_command();
void g29_eeprom_dump(); void g29_eeprom_dump();
void g29_compare_current_mesh_to_stored_mesh(); 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_clear(uint16_t bits[16], uint8_t x, uint8_t y);
void bit_set(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); 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_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)) #define MESH_Y_DIST ((float(UBL_MESH_MAX_Y) - float(UBL_MESH_MIN_Y)) / (float(UBL_MESH_NUM_Y_POINTS) - 1.0))
#if ENABLED(UBL_MESH_EDIT_ENABLED)
extern bool g26_debug_flag; extern bool g26_debug_flag;
#else
constexpr bool g26_debug_flag = false;
#endif
extern float last_specified_z; extern float last_specified_z;
extern float fade_scaling_factor_for_current_height; extern float fade_scaling_factor_for_current_height;
extern float z_values[UBL_MESH_NUM_X_POINTS][UBL_MESH_NUM_Y_POINTS]; 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_x_max = UBL_MESH_MAX_X,
mesh_y_max = UBL_MESH_MAX_Y, mesh_y_max = UBL_MESH_MAX_Y,
mesh_x_dist = MESH_X_DIST, mesh_x_dist = MESH_X_DIST,
mesh_y_dist = MESH_Y_DIST, mesh_y_dist = MESH_Y_DIST;
g29_correction_fade_height = 10.0,
#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 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 // 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 // is for the user. The second one is the one that is actually used
// again and again and again during the correction calculations. // 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 unsigned char padding[24]; // This is just to allow room to add state variables without
// changing the location of data structures in the EEPROM. // changing the location of data structures in the EEPROM.
@ -122,45 +127,45 @@
unified_bed_leveling(); unified_bed_leveling();
// ~unified_bed_leveling(); // No destructor because this object never goes away! // ~unified_bed_leveling(); // No destructor because this object never goes away!
void display_map(int); void display_map(const int);
void reset(); void reset();
void invalidate(); void invalidate();
void store_state(); void store_state();
void load_state(); void load_state();
void store_mesh(int); void store_mesh(const int16_t);
void load_mesh(int); void load_mesh(const int16_t);
bool sanity_check(); 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 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 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_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) { static int8_t get_cell_index_x(const float &x) {
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, (UBL_MESH_NUM_X_POINTS) - 1); // -1 is appropriate if we want all movement to the X_MAX 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 } // 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(float y) { static int8_t get_cell_index_y(const float &y) {
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, (UBL_MESH_NUM_Y_POINTS) - 1); // -1 is appropriate if we want all movement to the Y_MAX 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 } // 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 find_closest_x_index(float x) { static int8_t find_closest_x_index(const float &x) {
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 (px >= 0 && px < (UBL_MESH_NUM_X_POINTS)) ? px : -1; return (px >= 0 && px < (UBL_MESH_NUM_X_POINTS)) ? px : -1;
} }
int8_t find_closest_y_index(float y) { static int8_t find_closest_y_index(const float &y) {
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 (py >= 0 && py < (UBL_MESH_NUM_Y_POINTS)) ? py : -1; return (py >= 0 && py < (UBL_MESH_NUM_Y_POINTS)) ? py : -1;
} }
@ -174,14 +179,14 @@
* |<---delta_a---------->| * |<---delta_a---------->|
* *
* calc_z0 is the basis for all the Mesh Based correction. It is used to * 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. * multiplications.
*/ */
inline float calc_z0(float a0, float a1, float z1, float a2, float z2) { static FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
float delta_z = (z2 - z1); const float delta_z = (z2 - z1),
float delta_a = (a0 - a1) / (a2 - a1); delta_a = (a0 - a1) / (a2 - a1);
return z1 + delta_a * delta_z; 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 * the X index of the x0 intersection available and we don't want to perform any extra floating
* point operations. * 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) { 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("? in get_z_correction_along_horizontal_mesh_line_at_specific_X(x0=", x0);
SERIAL_ECHOPAIR(",x1_i=", x1_i); SERIAL_ECHOPAIR(",x1_i=", x1_i);
@ -203,18 +208,18 @@
return NAN; 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], z1 = z_values[x1_i][yi],
z2 = z_values[x1_i + 1][yi], z2 = z_values[x1_i + 1][yi],
dz = (z2 - z1); 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 // 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) { 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("? in get_z_correction_along_vertical_mesh_line_at_specific_X(y0=", y0);
SERIAL_ECHOPAIR(", x1_i=", xi); SERIAL_ECHOPAIR(", x1_i=", xi);
@ -224,12 +229,12 @@
return NAN; 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], z1 = z_values[xi][y1_i],
z2 = z_values[xi][y1_i + 1], z2 = z_values[xi][y1_i + 1],
dz = (z2 - z1); 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 * 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(float x0, float y0) { float get_z_correction(const float &x0, const float &y0) const {
int8_t cx = get_cell_index_x(x0), const int8_t cx = get_cell_index_x(RAW_X_POSITION(x0)),
cy = get_cell_index_y(y0); 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) { if (cx < 0 || cy < 0 || cx >= UBL_MESH_NUM_X_POINTS || cy >= UBL_MESH_NUM_Y_POINTS) {
@ -256,13 +261,13 @@
return 0.0; // this used to return state.z_offset return 0.0; // this used to return state.z_offset
} }
float z1 = calc_z0(x0, 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), z_values[cx][cy],
map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]); map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy]),
float z2 = calc_z0(x0, 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), z_values[cx][cy + 1],
map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]); map_x_index_to_bed_location(cx + 1), z_values[cx + 1][cy + 1]);
float z0 = calc_z0(y0, float z0 = calc_z0(RAW_Y_POSITION(y0),
map_y_index_to_bed_location(cy), z1, map_y_index_to_bed_location(cy), z1,
map_y_index_to_bed_location(cy + 1), z2); map_y_index_to_bed_location(cy + 1), z2);
@ -308,27 +313,36 @@
* factor is going to be the same as the last time the function calculated a value. If so, it just * factor is going to be the same as the last time the function calculated a value. If so, it just
* returns it. * returns it.
* *
* If it must do a calcuation, it will return a scaling factor of 0.0 if the UBL System is not active * It returns a scaling factor of 1.0 if UBL is inactive.
* or if the current Z Height is past the specified 'Fade Height' * 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) { #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
#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;
last_specified_z = current_z; 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 = fade_scaling_factor_for_current_height =
state.active && current_z < state.g29_correction_fade_height state.active && rz < state.g29_correction_fade_height
? 1.0 - (current_z * state.g29_fade_height_multiplier) ? 1.0 - (rz * state.g29_fade_height_multiplier)
: 0.0; : 0.0;
}
return fade_scaling_factor_for_current_height; 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 unified_bed_leveling ubl;
extern int ubl_eeprom_start; extern int ubl_eeprom_start;
#define UBL_LAST_EEPROM_INDEX (E2END - sizeof(unified_bed_leveling::state))
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL
#endif // UNIFIED_BED_LEVELING_H #endif // UNIFIED_BED_LEVELING_H

44
Marlin/UBL_Bed_Leveling.cpp
View File

@ -24,9 +24,20 @@
#include "math.h" #include "math.h"
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#include "UBL.h" #include "UBL.h"
#include "hex_print_routines.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 * 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 * still declare them within the .cpp file for bed leveling. But there is only one instance of
@ -51,17 +62,18 @@
} }
void unified_bed_leveling::store_state() { void unified_bed_leveling::store_state() {
int k = E2END - sizeof(ubl.state); const uint16_t i = UBL_LAST_EEPROM_INDEX;
eeprom_write_block((void *)&ubl.state, (void *)k, sizeof(ubl.state)); eeprom_write_block((void *)&ubl.state, (void *)i, sizeof(state));
} }
void unified_bed_leveling::load_state() { void unified_bed_leveling::load_state() {
int k = E2END - sizeof(ubl.state); const uint16_t i = UBL_LAST_EEPROM_INDEX;
eeprom_read_block((void *)&ubl.state, (void *)k, sizeof(ubl.state)); eeprom_read_block((void *)&ubl.state, (void *)i, sizeof(state));
if (sanity_check()) if (sanity_check())
SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n"); SERIAL_PROTOCOLLNPGM("?In load_state() sanity_check() failed.\n");
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
/** /**
* These lines can go away in a few weeks. They are just * These lines can go away in a few weeks. They are just
* to make sure people updating thier firmware won't be using * to make sure people updating thier firmware won't be using
@ -75,12 +87,11 @@
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(); store_state();
} }
#endif
} }
void unified_bed_leveling::load_mesh(int m) { void unified_bed_leveling::load_mesh(const int16_t m) {
int k = E2END - sizeof(ubl.state), int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
j = (k - ubl_eeprom_start) / sizeof(z_values);
if (m == -1) { if (m == -1) {
SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n"); SERIAL_PROTOCOLLNPGM("?No mesh saved in EEPROM. Zeroing mesh in memory.\n");
@ -93,7 +104,7 @@
return; 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)); eeprom_read_block((void *)&z_values , (void *)j, sizeof(z_values));
SERIAL_PROTOCOLPGM("Mesh loaded from slot "); SERIAL_PROTOCOLPGM("Mesh loaded from slot ");
@ -103,23 +114,22 @@
SERIAL_EOL; SERIAL_EOL;
} }
void unified_bed_leveling:: store_mesh(int m) { void unified_bed_leveling::store_mesh(const int16_t m) {
int k = E2END - sizeof(state), int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
j = (k - ubl_eeprom_start) / sizeof(z_values);
if (m < 0 || m >= j || ubl_eeprom_start <= 0) { if (m < 0 || m >= j || ubl_eeprom_start <= 0) {
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n"); SERIAL_PROTOCOLLNPGM("?EEPROM storage not available to load mesh.\n");
SERIAL_PROTOCOL(m); SERIAL_PROTOCOL(m);
SERIAL_PROTOCOLLNPGM(" mesh slots available.\n"); SERIAL_PROTOCOLLNPGM(" mesh slots available.\n");
SERIAL_PROTOCOLLNPAIR("E2END : ", E2END); SERIAL_PROTOCOLLNPAIR("E2END : ", E2END);
SERIAL_PROTOCOLLNPAIR("k : ", k); SERIAL_PROTOCOLLNPAIR("k : ", (int)UBL_LAST_EEPROM_INDEX);
SERIAL_PROTOCOLLNPAIR("j : ", j); SERIAL_PROTOCOLLNPAIR("j : ", j);
SERIAL_PROTOCOLLNPAIR("m : ", m); SERIAL_PROTOCOLLNPAIR("m : ", m);
SERIAL_EOL; SERIAL_EOL;
return; 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)); eeprom_write_block((const void *)&z_values, (void *)j, sizeof(z_values));
SERIAL_PROTOCOLPGM("Mesh saved in slot "); SERIAL_PROTOCOLPGM("Mesh saved in slot ");
@ -151,7 +161,7 @@
z_values[x][y] = NAN; 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; float f, current_xi, current_yi;
int8_t i, j; int8_t i, j;
UNUSED(map_type); UNUSED(map_type);
@ -287,9 +297,7 @@
error_flag++; error_flag++;
} }
const int k = E2END - sizeof(ubl.state), const int j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
j = (k - ubl_eeprom_start) / sizeof(z_values);
if (j < 1) { if (j < 1) {
SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n"); SERIAL_PROTOCOLLNPGM("?No EEPROM storage available for a mesh of this size.\n");
error_flag++; error_flag++;

235
Marlin/UBL_G29.cpp
View File

@ -20,12 +20,14 @@
* *
*/ */
#include "Marlin.h" #include "MarlinConfig.h"
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
//#include "vector_3.h" //#include "vector_3.h"
//#include "qr_solve.h" //#include "qr_solve.h"
#include "UBL.h" #include "UBL.h"
#include "Marlin.h"
#include "hex_print_routines.h" #include "hex_print_routines.h"
#include "configuration_store.h" #include "configuration_store.h"
#include "planner.h" #include "planner.h"
@ -49,12 +51,13 @@
#define DEPLOY_PROBE() set_probe_deployed(true) #define DEPLOY_PROBE() set_probe_deployed(true)
#define STOW_PROBE() set_probe_deployed(false) #define STOW_PROBE() set_probe_deployed(false)
bool ProbeStay = true; 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; constexpr float ubl_3_point_1_X = UBL_PROBE_PT_1_X,
float ubl_3_point_2_X = UBL_PROBE_PT_2_X; ubl_3_point_1_Y = UBL_PROBE_PT_1_Y,
float ubl_3_point_2_Y = UBL_PROBE_PT_2_Y; ubl_3_point_2_X = UBL_PROBE_PT_2_X,
float ubl_3_point_3_X = UBL_PROBE_PT_3_X; ubl_3_point_2_Y = UBL_PROBE_PT_2_Y,
float ubl_3_point_3_Y = UBL_PROBE_PT_3_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 SIZE_OF_LITTLE_RAISE 0
#define BIG_RAISE_NOT_NEEDED 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 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. // 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, static int g29_verbose_level = 0, phase_value = -1, repetition_cnt = 1,
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 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 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) #if ENABLED(ULTRA_LCD)
void lcd_setstatus(const char* message, bool persist); void lcd_setstatus(const char* message, bool persist);
#endif #endif
void gcode_G29() { void gcode_G29() {
mesh_index_pair location;
int j, k;
float Z1, Z2, Z3; float Z1, Z2, Z3;
g29_verbose_level = 0; // These may change, but let's get some reasonable values into them. g29_verbose_level = 0; // These may change, but let's get some reasonable values into them.
@ -331,7 +331,7 @@
if (code_seen('I')) { if (code_seen('I')) {
repetition_cnt = code_has_value() ? code_value_int() : 1; repetition_cnt = code_has_value() ? code_value_int() : 1;
while (repetition_cnt--) { 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) { if (location.x_index < 0) {
SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n"); SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
break; // No more invalid Mesh Points to populate break; // No more invalid Mesh Points to populate
@ -455,7 +455,7 @@
// If no repetition is specified, do the whole Mesh // If no repetition is specified, do the whole Mesh
if (!repeat_flag) repetition_cnt = 9999; if (!repeat_flag) repetition_cnt = 9999;
while (repetition_cnt--) { 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 if (location.x_index < 0) break; // No more invalid Mesh Points to populate
z_values[location.x_index][location.y_index] = height_value; z_values[location.x_index][location.y_index] = height_value;
} }
@ -534,8 +534,7 @@
if (code_seen('L')) { // Load Current Mesh Data if (code_seen('L')) { // Load Current Mesh Data
storage_slot = code_has_value() ? code_value_int() : ubl.state.eeprom_storage_slot; storage_slot = code_has_value() ? code_value_int() : ubl.state.eeprom_storage_slot;
k = E2END - sizeof(ubl.state); const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
j = (k - ubl_eeprom_start) / sizeof(z_values);
if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) { if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) {
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
@ -569,8 +568,7 @@
return; return;
} }
int k = E2END - sizeof(ubl.state), const int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(z_values);
j = (k - ubl_eeprom_start) / sizeof(z_values);
if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) { if (storage_slot < 0 || storage_slot >= j || ubl_eeprom_start <= 0) {
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
@ -702,9 +700,8 @@
* Probe all invalidated locations of the mesh that can be reached by the probe. * 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. * 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; mesh_index_pair location;
float xProbe, yProbe, measured_z;
ubl_has_control_of_lcd_panel++; 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 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(); restore_ubl_active_state_and_leave();
return; 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) { if (location.x_index >= 0 && location.y_index >= 0) {
xProbe = ubl.map_x_index_to_bed_location(location.x_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); 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) { 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."); SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
ubl_has_control_of_lcd_panel = false; ubl_has_control_of_lcd_panel = false;
goto LEAVE; 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; z_values[location.x_index][location.y_index] = measured_z + Z_PROBE_OFFSET_FROM_EXTRUDER;
} }
if (do_ubl_mesh_map) ubl.display_map(1); if (do_ubl_mesh_map) ubl.display_map(1);
} while (location.x_index >= 0 && location.y_index >= 0); } while (location.x_index >= 0 && location.y_index >= 0);
LEAVE: LEAVE:
@ -742,32 +741,27 @@
STOW_PROBE(); STOW_PROBE();
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
x_pos = constrain(x_pos - (X_PROBE_OFFSET_FROM_EXTRUDER), X_MIN_POS, X_MAX_POS); do_blocking_move_to_xy(
y_pos = constrain(y_pos - (Y_PROBE_OFFSET_FROM_EXTRUDER), Y_MIN_POS, Y_MAX_POS); 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)
do_blocking_move_to_xy(x_pos, y_pos); );
} }
vector tilt_mesh_based_on_3pts(float pt1, float pt2, float pt3) { vector_3 tilt_mesh_based_on_3pts(const float &pt1, const float &pt2, const float &pt3) {
vector v1, v2, normal;
float c, d, t; float c, d, t;
int i, j; int i, j;
v1.dx = (ubl_3_point_1_X - ubl_3_point_2_X); vector_3 v1 = vector_3( (ubl_3_point_1_X - ubl_3_point_2_X),
v1.dy = (ubl_3_point_1_Y - ubl_3_point_2_Y); (ubl_3_point_1_Y - ubl_3_point_2_Y),
v1.dz = (pt1 - pt2); (pt1 - pt2) ),
v2.dx = (ubl_3_point_3_X - ubl_3_point_2_X); v2 = vector_3( (ubl_3_point_3_X - ubl_3_point_2_X),
v2.dy = (ubl_3_point_3_Y - ubl_3_point_2_Y); (ubl_3_point_3_Y - ubl_3_point_2_Y),
v2.dz = (pt3 - pt2); (pt3 - pt2) ),
// do cross product normal = vector_3::cross(v1, v2);
normal.dx = v1.dy * v2.dz - v1.dz * v2.dy; // printf("[%f,%f,%f] ", normal.x, normal.y, normal.z);
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);
/** /**
* This code does two things. This vector is normal to the tilted plane. * 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 * 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 * as the sin() and cos() of the bed's tilt
*/ */
normal.dx /= normal.dz; const float inv_z = 1.0 / normal.z;
normal.dy /= normal.dz; normal.x *= inv_z;
normal.dz /= normal.dz; normal.y *= inv_z;
normal.z = 1.0;
// //
// All of 3 of these points should give us the same d constant // All of 3 of these points should give us the same d constant
// //
t = normal.dx * ubl_3_point_1_X + normal.dy * ubl_3_point_1_Y; t = normal.x * ubl_3_point_1_X + normal.y * ubl_3_point_1_Y;
d = t + normal.dz * pt1; d = t + normal.z * pt1;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 1st point: "); SERIAL_ECHOPGM("d from 1st point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
SERIAL_ECHOPGM(" c: "); SERIAL_ECHOPGM(" c: ");
SERIAL_ECHO_F(c, 6); SERIAL_ECHO_F(c, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.dx * ubl_3_point_2_X + normal.dy * ubl_3_point_2_Y; t = normal.x * ubl_3_point_2_X + normal.y * ubl_3_point_2_Y;
d = t + normal.dz * pt2; d = t + normal.z * pt2;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 2nd point: "); SERIAL_ECHOPGM("d from 2nd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
SERIAL_ECHOPGM(" c: "); SERIAL_ECHOPGM(" c: ");
SERIAL_ECHO_F(c, 6); SERIAL_ECHO_F(c, 6);
SERIAL_EOL; SERIAL_EOL;
t = normal.dx * ubl_3_point_3_X + normal.dy * ubl_3_point_3_Y; t = normal.x * ubl_3_point_3_X + normal.y * ubl_3_point_3_Y;
d = t + normal.dz * pt3; d = t + normal.z * pt3;
c = d - t; c = d - t;
SERIAL_ECHOPGM("d from 3rd point: "); SERIAL_ECHOPGM("d from 3rd point: ");
SERIAL_ECHO_F(d, 6); SERIAL_ECHO_F(d, 6);
@ -810,7 +805,7 @@
for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { 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; z_values[i][j] += c;
} }
} }
@ -829,7 +824,7 @@
return current_position[Z_AXIS]; 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++; 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 save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
@ -856,34 +851,34 @@
return abs(Z1 - Z2); 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) { 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) {
mesh_index_pair location;
float last_x, last_y, dx, dy,
xProbe, yProbe;
ubl_has_control_of_lcd_panel++; 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 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_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 { do {
if (do_ubl_mesh_map) ubl.display_map(1); 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 // It doesn't matter if the probe can not reach the
// NAN location. This is a manual probe. // NAN location. This is a manual probe.
if (location.x_index < 0 && location.y_index < 0) continue; if (location.x_index < 0 && location.y_index < 0) continue;
xProbe = ubl.map_x_index_to_bed_location(location.x_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); 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)) { 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."); SERIAL_PROTOCOLLNPGM("?Error: Attempt to probe off the bed.");
ubl_has_control_of_lcd_panel = false; ubl_has_control_of_lcd_panel = false;
goto LEAVE; goto LEAVE;
} }
dx = xProbe - last_x; const float dx = xProbe - last_x,
dy = yProbe - last_y; dy = yProbe - last_y;
if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED) if (HYPOT(dx, dy) < BIG_RAISE_NOT_NEEDED)
@ -891,9 +886,10 @@
else else
do_blocking_move_to_z(z_clearance); do_blocking_move_to_z(z_clearance);
do_blocking_move_to_xy(xProbe, yProbe);
last_x = xProbe; last_x = xProbe;
last_y = yProbe; last_y = yProbe;
do_blocking_move_to_xy(xProbe, yProbe);
wait_for_user = true; wait_for_user = true;
while (wait_for_user) { // we need the loop to move the nozzle based on the encoder wheel here! while (wait_for_user) { // we need the loop to move the nozzle based on the encoder wheel here!
@ -931,7 +927,7 @@
LEAVE: LEAVE:
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); 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() { bool g29_parameter_parsing() {
@ -983,7 +979,7 @@
ubl.store_state(); 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(); constant = code_value_float();
if (code_seen('D')) { // Disable the Unified Bed Leveling System if (code_seen('D')) { // Disable the Unified Bed Leveling System
@ -992,19 +988,17 @@
ubl.store_state(); ubl.store_state();
} }
if (code_seen('F')) { #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
ubl.state.g29_correction_fade_height = 10.00; if (code_seen('F') && code_has_value()) {
if (code_has_value()) { const float fh = code_value_float();
ubl.state.g29_correction_fade_height = code_value_float(); if (fh < 0.0 || fh > 100.0) {
ubl.state.g29_fade_height_multiplier = 1.0 / ubl.state.g29_correction_fade_height;
}
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"); 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; return UBL_ERR;
} }
ubl.state.g29_correction_fade_height = fh;
ubl.state.g29_fade_height_multiplier = 1.0 / fh;
} }
#endif
if ((repeat_flag = code_seen('R'))) { if ((repeat_flag = code_seen('R'))) {
repetition_cnt = code_has_value() ? code_value_int() : 9999; 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 * This function goes away after G29 debug is complete. But for right now, it is a handy
* routine to dump binary data structures. * routine to dump binary data structures.
*/ */
void dump(char *str, float f) { void dump(char * const str, const float &f) {
char *ptr; char *ptr;
SERIAL_PROTOCOL(str); SERIAL_PROTOCOL(str);
@ -1056,7 +1050,6 @@
} }
ubl_state_at_invocation = ubl.state.active; ubl_state_at_invocation = ubl.state.active;
ubl.state.active = 0; ubl.state.active = 0;
return;
} }
void restore_ubl_active_state_and_leave() { 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 * good to have the extra information. Soon... we prune this to just a few items
*/ */
void g29_what_command() { void g29_what_command() {
int k = E2END - ubl_eeprom_start; const uint16_t k = E2END - ubl_eeprom_start;
statistics_flag++; statistics_flag++;
SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version 1.00 "); SERIAL_PROTOCOLPGM("Unified Bed Leveling System Version 1.00 ");
if (ubl.state.active) ubl.state.active ? SERIAL_PROTOCOLCHAR('A') : SERIAL_PROTOCOLPGM("In");
SERIAL_PROTOCOLPGM("Active.\n"); SERIAL_PROTOCOLLNPGM("ctive.\n");
else
SERIAL_PROTOCOLPGM("Inactive.\n");
SERIAL_EOL;
delay(50); delay(50);
if (ubl.state.eeprom_storage_slot == 0xFFFF) { if (ubl.state.eeprom_storage_slot == -1)
SERIAL_PROTOCOLPGM("No Mesh Loaded."); SERIAL_PROTOCOLPGM("No Mesh Loaded.");
}
else { else {
SERIAL_PROTOCOLPGM("Mesh: "); SERIAL_PROTOCOLPGM("Mesh: ");
prt_hex_word(ubl.state.eeprom_storage_slot); prt_hex_word(ubl.state.eeprom_storage_slot);
SERIAL_PROTOCOLPGM(" Loaded."); SERIAL_PROTOCOLPGM(" Loaded.");
} }
SERIAL_EOL; SERIAL_EOL;
delay(50);
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height); SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height);
SERIAL_EOL; SERIAL_EOL;
#endif
idle();
SERIAL_PROTOCOLPGM("z_offset: "); SERIAL_PROTOCOLPGM("z_offset: ");
SERIAL_PROTOCOL_F(ubl.state.z_offset, 6); SERIAL_PROTOCOL_F(ubl.state.z_offset, 6);
@ -1111,28 +1098,20 @@
for (uint8_t i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { 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_PROTOCOL_F( ubl.map_x_index_to_bed_location(i), 1);
SERIAL_PROTOCOLPGM(" "); SERIAL_PROTOCOLPGM(" ");
delay(10);
} }
SERIAL_EOL; SERIAL_EOL;
delay(50);
idle();
SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: "); SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
for (uint8_t i = 0; i < UBL_MESH_NUM_Y_POINTS; i++) { 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_PROTOCOL_F( ubl.map_y_index_to_bed_location(i), 1);
SERIAL_PROTOCOLPGM(" "); SERIAL_PROTOCOLPGM(" ");
delay(10);
} }
SERIAL_EOL; SERIAL_EOL;
delay(50);
idle();
#if HAS_KILL #if HAS_KILL
SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN); SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN);
SERIAL_PROTOCOLLNPAIR(" state:", READ(KILL_PIN)); SERIAL_PROTOCOLLNPAIR(" state:", READ(KILL_PIN));
#endif #endif
delay(50);
idle();
SERIAL_EOL; SERIAL_EOL;
SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation); SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
@ -1142,54 +1121,39 @@
SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x"); SERIAL_PROTOCOLPGM("Free EEPROM space starts at: 0x");
prt_hex_word(ubl_eeprom_start); prt_hex_word(ubl_eeprom_start);
SERIAL_EOL; SERIAL_EOL;
delay(50);
idle();
SERIAL_PROTOCOLPGM("end of EEPROM : "); SERIAL_PROTOCOLPGM("end of EEPROM : ");
prt_hex_word(E2END); prt_hex_word(E2END);
SERIAL_EOL; SERIAL_EOL;
delay(50);
idle();
SERIAL_PROTOCOLLNPAIR("sizeof(ubl) : ", (int)sizeof(ubl)); SERIAL_PROTOCOLLNPAIR("sizeof(ubl) : ", (int)sizeof(ubl));
SERIAL_EOL; SERIAL_EOL;
SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values)); SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values));
SERIAL_EOL; SERIAL_EOL;
delay(50);
idle();
SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x"); SERIAL_PROTOCOLPGM("EEPROM free for UBL: 0x");
prt_hex_word(k); prt_hex_word(k);
SERIAL_EOL; SERIAL_EOL;
idle();
SERIAL_PROTOCOLPGM("EEPROM can hold 0x"); SERIAL_PROTOCOLPGM("EEPROM can hold 0x");
prt_hex_word(k / sizeof(z_values)); prt_hex_word(k / sizeof(z_values));
SERIAL_PROTOCOLLNPGM(" meshes.\n"); SERIAL_PROTOCOLLNPGM(" meshes.\n");
delay(50);
SERIAL_PROTOCOLPGM("sizeof(ubl.state) :"); SERIAL_PROTOCOLPGM("sizeof(ubl.state) :");
prt_hex_word(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_X_POINTS ", UBL_MESH_NUM_X_POINTS);
SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS); SERIAL_PROTOCOLPAIR("\nUBL_MESH_NUM_Y_POINTS ", UBL_MESH_NUM_Y_POINTS);
SERIAL_PROTOCOLPAIR("\nUBL_MESH_MIN_X ", UBL_MESH_MIN_X); 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_MIN_Y ", UBL_MESH_MIN_Y);
SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_X ", UBL_MESH_MAX_X);
SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y); SERIAL_PROTOCOLPAIR("\nUBL_MESH_MAX_Y ", UBL_MESH_MAX_Y);
delay(50);
idle();
SERIAL_PROTOCOLPGM("\nMESH_X_DIST "); SERIAL_PROTOCOLPGM("\nMESH_X_DIST ");
SERIAL_PROTOCOL_F(MESH_X_DIST, 6); SERIAL_PROTOCOL_F(MESH_X_DIST, 6);
SERIAL_PROTOCOLPGM("\nMESH_Y_DIST "); SERIAL_PROTOCOLPGM("\nMESH_Y_DIST ");
SERIAL_PROTOCOL_F(MESH_Y_DIST, 6); SERIAL_PROTOCOL_F(MESH_Y_DIST, 6);
SERIAL_EOL; SERIAL_EOL;
idle();
if (!ubl.sanity_check()) if (!ubl.sanity_check())
SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed."); SERIAL_PROTOCOLLNPGM("Unified Bed Leveling sanity checks passed.");
} }
@ -1205,7 +1169,7 @@
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOLNPGM("EEPROM Dump:"); SERIAL_ECHOLNPGM("EEPROM Dump:");
for (uint16_t i = 0; i < E2END + 1; i += 16) { for (uint16_t i = 0; i < E2END + 1; i += 16) {
if (i & 0x3 == 0) idle(); if (!(i & 0x3)) idle();
prt_hex_word(i); prt_hex_word(i);
SERIAL_ECHOPGM(": "); SERIAL_ECHOPGM(": ");
for (uint16_t j = 0; j < 16; j++) { for (uint16_t j = 0; j < 16; j++) {
@ -1217,7 +1181,6 @@
SERIAL_EOL; SERIAL_EOL;
} }
SERIAL_EOL; SERIAL_EOL;
return;
} }
/** /**
@ -1233,15 +1196,14 @@
} }
storage_slot = code_value_int(); storage_slot = code_value_int();
uint16_t k = E2END - sizeof(ubl.state), int16_t j = (UBL_LAST_EEPROM_INDEX - ubl_eeprom_start) / sizeof(tmp_z_values);
j = (k - ubl_eeprom_start) / sizeof(tmp_z_values);
if (storage_slot < 0 || storage_slot > j || ubl_eeprom_start <= 0) { if (storage_slot < 0 || storage_slot > j || ubl_eeprom_start <= 0) {
SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n"); SERIAL_PROTOCOLLNPGM("?EEPROM storage not available for use.\n");
return; 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)); eeprom_read_block((void *)&tmp_z_values, (void *)j, sizeof(tmp_z_values));
SERIAL_ECHOPAIR("Subtracting Mesh ", storage_slot); SERIAL_ECHOPAIR("Subtracting Mesh ", storage_slot);
@ -1254,23 +1216,19 @@
z_values[x][y] = z_values[x][y] - tmp_z_values[x][y]; 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; int i, j;
float f, px, py, mx, my, dx, dy, closest = 99999.99, float closest = 99999.99;
current_x, current_y, distance;
mesh_index_pair return_val; mesh_index_pair return_val;
return_val.x_index = return_val.y_index = -1; return_val.x_index = return_val.y_index = -1;
current_x = current_position[X_AXIS]; const float current_x = current_position[X_AXIS],
current_y = current_position[Y_AXIS]; current_y = current_position[Y_AXIS];
px = X; // Get our reference position. Either the nozzle or // Get our reference position. Either the nozzle or probe location.
py = Y; // the probe location. const float px = lx - (probe_as_reference ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
if (probe_as_reference) { py = ly - (probe_as_reference ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
px -= X_PROBE_OFFSET_FROM_EXTRUDER;
py -= Y_PROBE_OFFSET_FROM_EXTRUDER;
}
for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) { for (i = 0; i < UBL_MESH_NUM_X_POINTS; i++) {
for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) { for (j = 0; j < UBL_MESH_NUM_Y_POINTS; j++) {
@ -1282,8 +1240,8 @@
// We only get here if we found a Mesh Point of the specified type // We only get here if we found a Mesh Point of the specified type
mx = ubl.map_x_index_to_bed_location(i); // Check if we can probe this mesh location const float mx = LOGICAL_X_POSITION(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); 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. // 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 // We prune these out of the list and ignore them until the next Phase where we do the
@ -1293,13 +1251,9 @@
(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; ) continue;
dx = px - mx; // We can get to it. Let's see if it is the // We can get to it. Let's see if it is the closest location to the nozzle.
dy = py - my; // closest location to the nozzle. // Add in a weighting factor that considers the current location of the nozzle.
distance = HYPOT(dx, dy); const float distance = HYPOT(px - mx, py - my) + HYPOT(current_x - mx, current_y - my) * 0.01;
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
if (distance < closest) { if (distance < closest) {
closest = distance; // We found a closer location with closest = distance; // We found a closer location with
@ -1313,10 +1267,9 @@
return return_val; 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; mesh_index_pair location;
float xProbe, yProbe; uint16_t not_done[16];
uint16_t i, not_done[16];
int32_t round_off; int32_t round_off;
save_ubl_active_state_and_disable(); save_ubl_active_state_and_disable();
@ -1327,11 +1280,11 @@
#endif #endif
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); 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 { do {
if (do_ubl_mesh_map) ubl.display_map(1); 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 // It doesn't matter if the probe can not reach this
// location. This is a manual edit of the Mesh Point. // location. This is a manual edit of the Mesh Point.
if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points. if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points.
@ -1339,7 +1292,7 @@
bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a
// different location the next time through the loop // different location the next time through the loop
xProbe = ubl.map_x_index_to_bed_location(location.x_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); 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. 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, SERIAL_PROTOCOLLNPGM("?Error: Attempt to edit off the bed."); // This really can't happen, but for now,
@ -1406,7 +1359,7 @@
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); 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) #if ENABLED(ULTRA_LCD)
lcd_setstatus("Done Editing Mesh", true); lcd_setstatus("Done Editing Mesh", true);

197
Marlin/UBL_line_to_destination.cpp
View File

@ -29,35 +29,97 @@
#include <avr/io.h> #include <avr/io.h>
#include <math.h> #include <math.h>
extern float destination[XYZE];
extern void set_current_to_destination(); 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) { 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 * 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
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/ */
const float x_start = current_position[X_AXIS],
x_start = current_position[X_AXIS]; y_start = current_position[Y_AXIS],
y_start = current_position[Y_AXIS]; z_start = current_position[Z_AXIS],
z_start = current_position[Z_AXIS];
e_start = current_position[E_AXIS]; e_start = current_position[E_AXIS];
cell_start_xi = ubl.get_cell_index_x(x_start); const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_start)),
cell_start_yi = ubl.get_cell_index_y(y_start); cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_start)),
cell_dest_xi = ubl.get_cell_index_x(x_end); cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_end)),
cell_dest_yi = ubl.get_cell_index_y(y_end); cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_end));
if (g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPGM(" ubl_line_to_destination(xe="); SERIAL_ECHOPGM(" ubl_line_to_destination(xe=");
@ -68,7 +130,7 @@
SERIAL_ECHO(z_end); SERIAL_ECHO(z_end);
SERIAL_ECHOPGM(", ee="); SERIAL_ECHOPGM(", ee=");
SERIAL_ECHO(e_end); SERIAL_ECHO(e_end);
SERIAL_ECHOPGM(")\n"); SERIAL_ECHOLNPGM(")");
debug_current_and_destination((char*)"Start of ubl_line_to_destination()"); debug_current_and_destination((char*)"Start of ubl_line_to_destination()");
} }
@ -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. * 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); 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 *
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 ]),
(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 *
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]); (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.
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 * Debug code to use non-optimized get_z_correction() and to do a sanity check
@ -139,7 +199,7 @@
SERIAL_EOL; 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 * If part of the Mesh is undefined, it will show up as NAN
@ -167,31 +227,17 @@
* blocks of code: * blocks of code:
*/ */
dx = x_end - x_start; const float dx = x_end - x_start,
dy = y_end - y_start; dy = y_end - y_start;
const int left_flag = dx < 0.0 ? 1 : 0, const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 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 const float adx = left_flag ? -dx : dx,
dxi = -1; ady = down_flag ? -dy : dy;
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;
}
if (cell_start_xi == cell_dest_xi) dxi = 0; const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
if (cell_start_yi == cell_dest_yi) dyi = 0; dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
/** /**
* Compute the scaling factor for the extruder for each partial move. * Compute the scaling factor for the extruder for each partial move.
@ -204,22 +250,20 @@
const bool use_x_dist = adx > ady; 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,
e_position = e_end - e_start;
e_normalized_dist = e_position / on_axis_distance;
z_position = z_end - z_start; z_position = z_end - z_start;
const float e_normalized_dist = e_position / on_axis_distance,
z_normalized_dist = z_position / on_axis_distance; z_normalized_dist = z_position / on_axis_distance;
const bool inf_normalized_flag = e_normalized_dist == INFINITY || e_normalized_dist == -INFINITY; int current_xi = cell_start_xi, current_yi = cell_start_yi;
current_xi = cell_start_xi; const float m = dy / dx,
current_yi = cell_start_yi;
m = dy / dx;
c = y_start - m * x_start; 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 * 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 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;
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 * 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! * else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line * 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 * Debug code to use non-optimized get_z_correction() and to do a sanity check
@ -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 * If part of the Mesh is undefined, it will show up as NAN
@ -272,7 +316,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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 * 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. // 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;
next_mesh_line_x = mesh_index_to_x_location[current_xi]; 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 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 * Debug code to use non-optimized get_z_correction() and to do a sanity check
@ -357,7 +401,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; 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 * 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; int xi_cnt = cell_start_xi - cell_dest_xi,
if (xi_cnt < 0) xi_cnt = -xi_cnt;
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; if (yi_cnt < 0) yi_cnt = -yi_cnt;
current_xi += left_flag; current_xi += left_flag;
@ -407,10 +451,9 @@
while (xi_cnt > 0 || yi_cnt > 0) { while (xi_cnt > 0 || yi_cnt > 0) {
next_mesh_line_x = mesh_index_to_x_location[current_xi + dxi]; const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_x_location[current_xi + dxi]),
next_mesh_line_y = mesh_index_to_y_location[current_yi + dyi]; 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
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 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 // 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 // detected this as a vertical line move up above and we wouldn't
@ -420,7 +463,7 @@
// //
// Yes! Crossing a Y Mesh Line next // 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 * Debug code to use non-optimized get_z_correction() and to do a sanity check
@ -471,7 +514,7 @@
// //
// Yes! Crossing a X Mesh Line next // 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 * Debug code to use non-optimized get_z_correction() and to do a sanity check
@ -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 * If part of the Mesh is undefined, it will show up as NAN

80
Marlin/configuration_store.cpp
View File

@ -879,7 +879,7 @@ void Config_Postprocess() {
} }
else { else {
ubl.reset(); ubl.reset();
SERIAL_ECHOPGM("UBL System reset() \n"); SERIAL_ECHOLNPGM("UBL System reset()");
} }
#endif #endif
} }
@ -1178,42 +1178,6 @@ void Config_ResetDefault() {
SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS]); SERIAL_ECHOPAIR(" Z", home_offset[Z_AXIS]);
SERIAL_EOL; SERIAL_EOL;
#endif #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 #if HOTENDS > 1
CONFIG_ECHO_START; CONFIG_ECHO_START;
@ -1233,6 +1197,7 @@ void Config_ResetDefault() {
#endif #endif
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
if (!forReplay) { if (!forReplay) {
SERIAL_ECHOLNPGM("Mesh Bed Leveling:"); SERIAL_ECHOLNPGM("Mesh Bed Leveling:");
CONFIG_ECHO_START; CONFIG_ECHO_START;
@ -1248,12 +1213,53 @@ void Config_ResetDefault() {
SERIAL_EOL; 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 #elif HAS_ABL
if (!forReplay) { if (!forReplay) {
SERIAL_ECHOLNPGM("Auto Bed Leveling:"); SERIAL_ECHOLNPGM("Auto Bed Leveling:");
CONFIG_ECHO_START; CONFIG_ECHO_START;
} }
SERIAL_ECHOLNPAIR(" M420 S", planner.abl_enabled ? 1 : 0); SERIAL_ECHOLNPAIR(" M420 S", planner.abl_enabled ? 1 : 0);
#endif #endif
#if ENABLED(DELTA) #if ENABLED(DELTA)

6
Marlin/planner.cpp
View File

@ -530,7 +530,7 @@ void Planner::check_axes_activity() {
#endif #endif
} }
#if PLANNER_LEVELING #if PLANNER_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)
/** /**
* lx, ly, lz - logical (cartesian, not delta) positions in mm * lx, ly, lz - logical (cartesian, not delta) positions in mm
*/ */
@ -634,7 +634,7 @@ void Planner::check_axes_activity() {
#endif #endif
} }
#endif // PLANNER_LEVELING #endif // PLANNER_LEVELING && !AUTO_BED_LEVELING_UBL
/** /**
* Planner::_buffer_line * 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]) { 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] }; float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
apply_leveling(lpos); apply_leveling(lpos);
#else #else

8
Marlin/planner.h
View File

@ -244,7 +244,7 @@ class Planner {
static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); } 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_X float lx
#define ARG_Y float ly #define ARG_Y float ly
@ -300,7 +300,7 @@ class Planner {
* extruder - target extruder * 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) { 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); apply_leveling(lx, ly, lz);
#endif #endif
_buffer_line(lx, ly, lz, e, fr_mm_s, extruder); _buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
@ -316,7 +316,7 @@ class Planner {
* extruder - target extruder * extruder - target extruder
*/ */
static FORCE_INLINE void buffer_line_kinematic(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t 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] }; float lpos[XYZ] = { ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS] };
apply_leveling(lpos); apply_leveling(lpos);
#else #else
@ -340,7 +340,7 @@ class Planner {
* Clears previous speed values. * Clears previous speed values.
*/ */
static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { 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); apply_leveling(lx, ly, lz);
#endif #endif
_set_position_mm(lx, ly, lz, e); _set_position_mm(lx, ly, lz, e);