Patches to bring UBL closer to compliance

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

View File

@ -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

View File

@ -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

View File

@ -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)

View File

@ -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

View File

@ -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++;

View File

@ -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);

View File

@ -29,35 +29,97 @@
#include <avr/io.h>
#include <math.h>
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

View File

@ -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)

View File

@ -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

View File

@ -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);