Merge pull request #6099 from thinkyhead/rc_cleanup_ubl_1

Patch till UBL is integrated with planner-based leveling
This commit is contained in:
Scott Lahteine 2017-03-24 02:07:55 -05:00 committed by GitHub
commit 4433b63d7a
37 changed files with 553 additions and 593 deletions

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

View file

@ -862,6 +862,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -24,25 +24,27 @@
* 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
#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" #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_MESH_EDIT_ENABLED)
#include "Configuration.h"
#include "planner.h"
#include "stepper.h"
#include "temperature.h"
#include "UBL.h"
#include "ultralcd.h"
#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_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
@ -50,64 +52,64 @@
/** /**
* Roxy's G26 Mesh Validation Tool * Roxy's G26 Mesh Validation Tool
* *
* G26 Is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System. * 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 * 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 * 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 * 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). * 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 * 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 * 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 * 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. * 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. * 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 * C Current When searching for Mesh Intersection points to draw, use the current nozzle location
* as the base for any distance comparison. * as the base for any distance comparison.
* *
* D Disable Disable the Unified Bed Leveling System. In the normal case the user is invoking this * 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 * 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 * 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 * alters the command's normal behaviour and disables the Unified Bed Leveling System even if
* it is on. * it is on.
* *
* H # Hotend Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed. * 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 * 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 * 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 * '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. * 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. * 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. * 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 * 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 * 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. * 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 * 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 * 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 * 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. * 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 * 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 * 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 * 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 * 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 * 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. * 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 * 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 * 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 * 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. * deviation from the normal behaviour on a 10 x 10 Mesh.
* *
* X # X coordinate Specify the starting location of the drawing activity. * X # X coordinate Specify the starting location of the drawing activity.
* *
* Y # Y 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); 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

View file

@ -40,7 +40,6 @@
#include "fastio.h" #include "fastio.h"
#include "utility.h" #include "utility.h"
#ifdef USBCON #ifdef USBCON
#include "HardwareSerial.h" #include "HardwareSerial.h"
#if ENABLED(BLUETOOTH) #if ENABLED(BLUETOOTH)

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;
@ -2009,7 +2011,7 @@ static void clean_up_after_endstop_or_probe_move() {
safe_delay(375); safe_delay(375);
} }
FORCE_INLINE void set_bltouch_deployed(const bool &deploy) { void set_bltouch_deployed(const bool deploy) {
bltouch_command(deploy ? BLTOUCH_DEPLOY : BLTOUCH_STOW); bltouch_command(deploy ? BLTOUCH_DEPLOY : BLTOUCH_STOW);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
@ -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.
@ -2284,7 +2286,7 @@ static void clean_up_after_endstop_or_probe_move() {
mbl.set_active(enable && mbl.has_mesh()); mbl.set_active(enable && mbl.has_mesh());
if (enable) planner.unapply_leveling(current_position); if (enable && mbl.has_mesh()) planner.unapply_leveling(current_position);
} }
#elif HAS_ABL && !ENABLED(AUTO_BED_LEVELING_UBL) #elif HAS_ABL && !ENABLED(AUTO_BED_LEVELING_UBL)
@ -2309,7 +2311,8 @@ static void clean_up_after_endstop_or_probe_move() {
planner.unapply_leveling(current_position); planner.unapply_leveling(current_position);
} }
#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;
@ -8528,7 +8541,7 @@ void process_next_command() {
gcode_G28(); gcode_G28();
break; break;
#if PLANNER_LEVELING || HAS_ABL #if PLANNER_LEVELING
case 29: // G29 Detailed Z probe, probes the bed at 3 or more points, case 29: // G29 Detailed Z probe, probes the bed at 3 or more points,
// or provides access to the UBL System if enabled. // or provides access to the UBL System if enabled.
gcode_G29(); gcode_G29();
@ -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)
@ -9584,7 +9592,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
float normalized_dist, end[XYZE]; float normalized_dist, end[XYZE];
// Split at the left/front border of the right/top square // Split at the left/front border of the right/top square
int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
if (cx2 != cx1 && TEST(x_splits, gcx)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx)); destination[X_AXIS] = LOGICAL_X_POSITION(mbl.get_probe_x(gcx));
@ -9647,7 +9655,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
float normalized_dist, end[XYZE]; float normalized_dist, end[XYZE];
// Split at the left/front border of the right/top square // Split at the left/front border of the right/top square
int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
if (cx2 != cx1 && TEST(x_splits, gcx)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx); destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx);
@ -10288,7 +10296,7 @@ void prepare_move_to_destination() {
float calculate_volumetric_multiplier(float diameter) { float calculate_volumetric_multiplier(float diameter) {
if (!volumetric_enabled || diameter == 0) return 1.0; if (!volumetric_enabled || diameter == 0) return 1.0;
return 1.0 / (M_PI * diameter * 0.5 * diameter * 0.5); return 1.0 / (M_PI * sq(diameter * 0.5));
} }
void calculate_volumetric_multipliers() { void calculate_volumetric_multipliers() {

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))
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 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,
g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// point divide. So, we keep this number in both forms. The first float g29_correction_fade_height = 10.0,
// is for the user. The second one is the one that is actually used g29_fade_height_multiplier = 1.0 / 10.0; // It is cheaper to do a floating point multiply than a floating
// again and again and again during the correction calculations. // 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 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,15 +261,15 @@
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);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { 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 * 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 {
fade_scaling_factor_for_current_height = const float rz = RAW_Z_POSITION(lz);
state.active && current_z < state.g29_correction_fade_height if (last_specified_z != rz) {
? 1.0 - (current_z * state.g29_fade_height_multiplier) last_specified_z = rz;
: 0.0; fade_scaling_factor_for_current_height =
return 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 unified_bed_leveling ubl;
extern int ubl_eeprom_start; 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 #endif // UNIFIED_BED_LEVELING_H

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,36 +62,36 @@
} }
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 /**
* to make sure people updating thier firmware won't be using * These lines can go away in a few weeks. They are just
* an incomplete Bed_Leveling.state structure. For speed * to make sure people updating thier firmware won't be using
* we now multiply by the inverse of the Fade Height instead of * an incomplete Bed_Leveling.state structure. For speed
* dividing by it. Soon... all of the old structures will be * we now multiply by the inverse of the Fade Height instead of
* updated, but until then, we try to ease the transition * dividing by it. Soon... all of the old structures will be
* for our Beta testers. * 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; if (ubl.state.g29_fade_height_multiplier != 1.0 / ubl.state.g29_correction_fade_height) {
store_state(); 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) { 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++;

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
@ -409,8 +409,8 @@
SERIAL_ECHOPAIR(",", y_pos); SERIAL_ECHOPAIR(",", y_pos);
SERIAL_PROTOCOLLNPGM(")\n"); SERIAL_PROTOCOLLNPGM(")\n");
} }
probe_entire_mesh( x_pos+X_PROBE_OFFSET_FROM_EXTRUDER, y_pos+Y_PROBE_OFFSET_FROM_EXTRUDER, 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')); code_seen('O') || code_seen('M'), code_seen('E'));
break; break;
// //
// Manually Probe Mesh in areas that can not be reached by the probe // 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 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");
@ -691,7 +689,7 @@
z_values[x][y] -= mean + constant; 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 x = 0; x < UBL_MESH_NUM_X_POINTS; x++)
for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++) for (uint8_t y = 0; y < UBL_MESH_NUM_Y_POINTS; y++)
if (!isnan(z_values[x][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. * 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,44 +851,45 @@
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)
do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE); do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
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; 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) { #endif
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;
}
}
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;
delay(50);
SERIAL_PROTOCOLPAIR("g29_correction_fade_height : ", ubl.state.g29_correction_fade_height );
SERIAL_EOL; 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_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,24 +1240,20 @@
// 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
// manual nozzle probing. // manual nozzle probing.
if (probe_as_reference && 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; ) 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,8 +1292,8 @@
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,
ubl_has_control_of_lcd_panel = false; // Let's do the check. ubl_has_control_of_lcd_panel = false; // Let's do the check.
@ -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);

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],
y_start = current_position[Y_AXIS],
z_start = current_position[Z_AXIS],
e_start = current_position[E_AXIS];
x_start = current_position[X_AXIS]; const int cell_start_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_start)),
y_start = current_position[Y_AXIS]; cell_start_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_start)),
z_start = current_position[Z_AXIS]; cell_dest_xi = ubl.get_cell_index_x(RAW_X_POSITION(x_end)),
e_start = current_position[E_AXIS]; cell_dest_yi = ubl.get_cell_index_y(RAW_Y_POSITION(y_end));
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);
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()");
} }
@ -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) { 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. // a reasonable correction would be.
planner.buffer_line(x_end, y_end, z_end + ubl.state.z_offset, e_end, feed_rate, extruder); 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. * 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 *
(z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
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]);
// 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
@ -126,7 +186,7 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"FINAL_MOVE: z_correction()"); debug_current_and_destination((char*)"FINAL_MOVE: z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); if (isnan(z0)) SERIAL_ECHO(" z0==NAN ");
@ -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,
z_position = z_end - z_start;
e_position = e_end - e_start; const float e_normalized_dist = e_position / on_axis_distance,
e_normalized_dist = e_position / on_axis_distance; z_normalized_dist = z_position / on_axis_distance;
z_position = z_end - z_start; int current_xi = cell_start_xi, current_yi = cell_start_yi;
z_normalized_dist = z_position / on_axis_distance;
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; const bool inf_normalized_flag = NEAR_ZERO(on_axis_distance),
current_yi = cell_start_yi; inf_m_flag = NEAR_ZERO(dx);
m = dy / dx;
c = y_start - m * x_start;
const bool inf_m_flag = (m == INFINITY || m == -INFINITY);
/** /**
* 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
@ -247,7 +291,7 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"VERTICAL z_correction()"); debug_current_and_destination((char*)"VERTICAL z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); 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 * 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
@ -332,7 +376,7 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"HORIZONTAL z_correction()"); debug_current_and_destination((char*)"HORIZONTAL z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); if (isnan(z0)) SERIAL_ECHO(" z0==NAN ");
@ -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,20 +451,19 @@
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 // be down here doing a generic type of move.
// 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 if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
// //
// Yes! Crossing a Y Mesh Line next // Yes! Crossing a Y Mesh Line next
// //
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
@ -428,7 +471,7 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"General_1: z_correction()"); debug_current_and_destination((char*)"General_1: z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); if (isnan(z0)) SERIAL_ECHO(" z0==NAN ");
@ -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
@ -479,7 +522,7 @@
*/ */
/* /*
z_optimized = z0; 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)) { if (fabs(z_optimized - z0) > .01 || isnan(z0) || isnan(z_optimized)) {
debug_current_and_destination((char*)"General_2: z_correction()"); debug_current_and_destination((char*)"General_2: z_correction()");
if (isnan(z0)) SERIAL_ECHO(" z0==NAN "); 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 * 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) #if ENABLED(AUTO_BED_LEVELING_UBL)
ubl_eeprom_start = (eeprom_index + 32) & 0xFFF8; // Pad the end of configuration data so it ubl_eeprom_start = (eeprom_index + 32) & 0xFFF8; // Pad the end of configuration data so it
// can float up or down a little bit without // can float up or down a little bit without
// disrupting the Unified Bed Leveling data // disrupting the Unified Bed Leveling data
ubl.load_state(); ubl.load_state();
SERIAL_ECHOPGM(" UBL "); SERIAL_ECHOPGM(" UBL ");
@ -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)

View file

@ -863,6 +863,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -846,6 +846,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -846,6 +846,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -855,6 +855,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -857,6 +857,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -892,6 +892,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -863,6 +863,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -863,6 +863,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -863,6 +863,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -862,6 +862,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -878,6 +878,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -884,6 +884,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -855,6 +855,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -863,6 +863,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -968,6 +968,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -954,6 +954,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -958,6 +958,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -957,6 +957,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -967,6 +967,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -866,6 +866,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -859,6 +859,7 @@
#define UBL_PROBE_PT_2_Y 20 #define UBL_PROBE_PT_2_Y 20
#define UBL_PROBE_PT_3_X 180 #define UBL_PROBE_PT_3_X 180
#define UBL_PROBE_PT_3_Y 20 #define UBL_PROBE_PT_3_Y 20
#define UBL_MESH_EDIT_ENABLED // Enable G26 mesh editing
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)

View file

@ -87,12 +87,12 @@
} }
int8_t probe_index_x(const float &x) const { int8_t probe_index_x(const float &x) const {
int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST)); int8_t px = (x - (MESH_MIN_X) + 0.5 * (MESH_X_DIST)) * (1.0 / (MESH_X_DIST));
return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1; return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1;
} }
int8_t probe_index_y(const float &y) const { int8_t probe_index_y(const float &y) const {
int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST)); int8_t py = (y - (MESH_MIN_Y) + 0.5 * (MESH_Y_DIST)) * (1.0 / (MESH_Y_DIST));
return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1; return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1;
} }

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

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

View file

@ -535,8 +535,7 @@ void Stepper::isr() {
// If a minimum pulse time was specified get the CPU clock // If a minimum pulse time was specified get the CPU clock
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_CODE #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_CODE
static uint32_t pulse_start; uint32_t pulse_start = TCNT0;
pulse_start = TCNT0;
#endif #endif
#if HAS_X_STEP #if HAS_X_STEP
@ -802,8 +801,7 @@ void Stepper::isr() {
for (uint8_t i = 0; i < step_loops; i++) { for (uint8_t i = 0; i < step_loops; i++) {
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_E #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_E
static uint32_t pulse_start; uint32_t pulse_start = TCNT0;
pulse_start = TCNT0;
#endif #endif
START_E_PULSE(0); START_E_PULSE(0);
@ -1232,55 +1230,53 @@ void Stepper::report_positions() {
#if ENABLED(BABYSTEPPING) #if ENABLED(BABYSTEPPING)
#define CYCLES_EATEN_BY_BABYSTEP 60 #define CYCLES_EATEN_BY_BABYSTEP 60
#define _ENABLE(axis) enable_## axis() #define _ENABLE(axis) enable_## axis()
#define _READ_DIR(AXIS) AXIS ##_DIR_READ #define _READ_DIR(AXIS) AXIS ##_DIR_READ
#define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR #define _INVERT_DIR(AXIS) INVERT_## AXIS ##_DIR
#define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true) #define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
#define _SAVE_START (pulse_start = TCNT0)
#define _PULSE_WAIT while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
#else
#define _SAVE_START NOOP
#define _PULSE_WAIT NOOP
#endif
#define START_BABYSTEP_AXIS(AXIS, INVERT) { \ #define START_BABYSTEP_AXIS(AXIS, INVERT) { \
old_dir = _READ_DIR(AXIS); \
_SAVE_START; \
_APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \ _APPLY_DIR(AXIS, _INVERT_DIR(AXIS)^direction^INVERT); \
_APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \ _APPLY_STEP(AXIS)(!_INVERT_STEP_PIN(AXIS), true); \
} }
#define STOP_BABYSTEP_AXIS(AXIS) { \ #define STOP_BABYSTEP_AXIS(AXIS) { \
_PULSE_WAIT; \
_APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \ _APPLY_STEP(AXIS)(_INVERT_STEP_PIN(AXIS), true); \
_APPLY_DIR(AXIS, old_pin); \ _APPLY_DIR(AXIS, old_dir); \
} }
// MUST ONLY BE CALLED BY AN ISR, // MUST ONLY BE CALLED BY AN ISR,
// No other ISR should ever interrupt this! // No other ISR should ever interrupt this!
void Stepper::babystep(const AxisEnum axis, const bool direction) { void Stepper::babystep(const AxisEnum axis, const bool direction) {
cli(); cli();
static uint8_t old_pin; uint8_t old_dir;
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP #if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
static uint32_t pulse_start; uint32_t pulse_start;
#endif #endif
switch (axis) { switch (axis) {
case X_AXIS: case X_AXIS:
_ENABLE(x); _ENABLE(x);
old_pin = _READ_DIR(X);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
pulse_start = TCNT0;
#endif
START_BABYSTEP_AXIS(X, false); START_BABYSTEP_AXIS(X, false);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
#endif
STOP_BABYSTEP_AXIS(X); STOP_BABYSTEP_AXIS(X);
break; break;
case Y_AXIS: case Y_AXIS:
_ENABLE(y); _ENABLE(y);
old_pin = _READ_DIR(Y);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
pulse_start = TCNT0;
#endif
START_BABYSTEP_AXIS(Y, false); START_BABYSTEP_AXIS(Y, false);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
#endif
STOP_BABYSTEP_AXIS(Y); STOP_BABYSTEP_AXIS(Y);
break; break;
@ -1289,14 +1285,7 @@ void Stepper::report_positions() {
#if DISABLED(DELTA) #if DISABLED(DELTA)
_ENABLE(z); _ENABLE(z);
old_pin = _READ_DIR(Z);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
pulse_start = TCNT0;
#endif
START_BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z); START_BABYSTEP_AXIS(Z, BABYSTEP_INVERT_Z);
#if STEP_PULSE_CYCLES > CYCLES_EATEN_BY_BABYSTEP
while ((uint32_t)(TCNT0 - pulse_start) < STEP_PULSE_CYCLES - CYCLES_EATEN_BY_BABYSTEP) { /* nada */ }
#endif
STOP_BABYSTEP_AXIS(Z); STOP_BABYSTEP_AXIS(Z);
#else // DELTA #else // DELTA
@ -1340,7 +1329,7 @@ void Stepper::report_positions() {
sei(); sei();
} }
#endif //BABYSTEPPING #endif // BABYSTEPPING
/** /**
* Software-controlled Stepper Motor Current * Software-controlled Stepper Motor Current

View file

@ -860,7 +860,7 @@ void kill_screen(const char* lcd_msg) {
static void _lcd_mesh_fine_tune(const char* msg) { static void _lcd_mesh_fine_tune(const char* msg) {
static millis_t next_click = 0; static millis_t next_click = 0;
int16_t last_digit, movement; int16_t last_digit;
int32_t rounded; int32_t rounded;
defer_return_to_status = true; defer_return_to_status = true;

View file

@ -66,16 +66,16 @@ vector_3 vector_3::get_normal() {
float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); } float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
void vector_3::normalize() { void vector_3::normalize() {
float length = get_length(); const float inv_length = 1.0 / get_length();
x /= length; x *= inv_length;
y /= length; y *= inv_length;
z /= length; z *= inv_length;
} }
void vector_3::apply_rotation(matrix_3x3 matrix) { void vector_3::apply_rotation(matrix_3x3 matrix) {
float resultX = x * matrix.matrix[3 * 0 + 0] + y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0]; const float resultX = x * matrix.matrix[3 * 0 + 0] + y * matrix.matrix[3 * 1 + 0] + z * matrix.matrix[3 * 2 + 0],
float resultY = x * matrix.matrix[3 * 0 + 1] + y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1]; resultY = x * matrix.matrix[3 * 0 + 1] + y * matrix.matrix[3 * 1 + 1] + z * matrix.matrix[3 * 2 + 1],
float resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2]; resultZ = x * matrix.matrix[3 * 0 + 2] + y * matrix.matrix[3 * 1 + 2] + z * matrix.matrix[3 * 2 + 2];
x = resultX; x = resultX;
y = resultY; y = resultY;
z = resultZ; z = resultZ;
@ -92,7 +92,7 @@ void vector_3::debug(const char title[]) {
SERIAL_EOL; SERIAL_EOL;
} }
void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) { void apply_rotation_xyz(matrix_3x3 matrix, float &x, float &y, float &z) {
vector_3 vector = vector_3(x, y, z); vector_3 vector = vector_3(x, y, z);
vector.apply_rotation(matrix); vector.apply_rotation(matrix);
x = vector.x; x = vector.x;
@ -144,9 +144,9 @@ matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
void matrix_3x3::debug(const char title[]) { void matrix_3x3::debug(const char title[]) {
SERIAL_PROTOCOLLN(title); SERIAL_PROTOCOLLN(title);
int count = 0; uint8_t count = 0;
for (int i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) { for (uint8_t j = 0; j < 3; j++) {
if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+'); if (matrix[count] >= 0.0) SERIAL_PROTOCOLCHAR('+');
SERIAL_PROTOCOL_F(matrix[count], 6); SERIAL_PROTOCOL_F(matrix[count], 6);
SERIAL_PROTOCOLCHAR(' '); SERIAL_PROTOCOLCHAR(' ');