Ubl delta fixes and improvements (#6941)
* Change all direct changes of ubl.state.active to set_bed_leveling_enabled() which handles apply/unapply leveling to maintain current_position consistency. Fix invalidation of UBL mesh to invalid unreachable mesh points as well (delta corners). Fix UBL_DELTA unapply_leveling logic and when it gets applied, including fade_height changes. Add optional M114 D for detailed position information, disabled from compilation by default (M114_DETAIL). * UBL_DELTA raw and inline kinematics * UBL planner fall through fix * consistent variable names * Cleanup orphaned code and whitespace changes. Use _O2. * compile warnings cleanup * Remove redundant #ifdef condition
This commit is contained in:
oldmcg
Roxy-3D
parent
862def2ef4
commit
9f295581f0
@ -718,7 +718,7 @@
|
||||
/**
|
||||
* Wait until all parameters are verified before altering the state!
|
||||
*/
|
||||
state.active = !parser.seen('D');
|
||||
set_bed_leveling_enabled(!parser.seen('D'));
|
||||
|
||||
return UBL_OK;
|
||||
}
|
||||
@ -734,7 +734,7 @@
|
||||
* wait for them to get up to temperature.
|
||||
*/
|
||||
bool unified_bed_leveling::turn_on_heaters() {
|
||||
millis_t next;
|
||||
millis_t next = millis() + 5000UL;
|
||||
#if HAS_TEMP_BED
|
||||
#if ENABLED(ULTRA_LCD)
|
||||
if (g26_bed_temp > 25) {
|
||||
@ -743,7 +743,6 @@
|
||||
#endif
|
||||
has_control_of_lcd_panel = true;
|
||||
thermalManager.setTargetBed(g26_bed_temp);
|
||||
next = millis() + 5000UL;
|
||||
while (abs(thermalManager.degBed() - g26_bed_temp) > 3) {
|
||||
if (ubl_lcd_clicked()) return exit_from_g26();
|
||||
if (PENDING(millis(), next)) {
|
||||
|
||||
@ -699,7 +699,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis);
|
||||
#endif
|
||||
|
||||
void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
|
||||
static void report_current_position();
|
||||
void report_current_position();
|
||||
void report_current_position_detail();
|
||||
|
||||
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
||||
void print_xyz(const char* prefix, const char* suffix, const float x, const float y, const float z) {
|
||||
@ -1536,14 +1537,21 @@ inline void set_destination_to_current() { COPY(destination, current_position);
|
||||
if (DEBUGGING(LEVELING)) DEBUG_POS("prepare_uninterpolated_move_to_destination", destination);
|
||||
#endif
|
||||
|
||||
if ( current_position[X_AXIS] == destination[X_AXIS]
|
||||
&& current_position[Y_AXIS] == destination[Y_AXIS]
|
||||
&& current_position[Z_AXIS] == destination[Z_AXIS]
|
||||
&& current_position[E_AXIS] == destination[E_AXIS]
|
||||
) return;
|
||||
|
||||
refresh_cmd_timeout();
|
||||
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
|
||||
|
||||
#if UBL_DELTA
|
||||
// ubl segmented line will do z-only moves in single segment
|
||||
ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
|
||||
#else
|
||||
if ( current_position[X_AXIS] == destination[X_AXIS]
|
||||
&& current_position[Y_AXIS] == destination[Y_AXIS]
|
||||
&& current_position[Z_AXIS] == destination[Z_AXIS]
|
||||
&& current_position[E_AXIS] == destination[E_AXIS]
|
||||
) return;
|
||||
|
||||
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
|
||||
#endif
|
||||
|
||||
set_current_to_destination();
|
||||
}
|
||||
#endif // IS_KINEMATIC
|
||||
@ -2345,18 +2353,21 @@ static void clean_up_after_endstop_or_probe_move() {
|
||||
if (enabling) planner.unapply_leveling(current_position);
|
||||
|
||||
#elif ENABLED(AUTO_BED_LEVELING_UBL)
|
||||
|
||||
#if PLANNER_LEVELING
|
||||
|
||||
if (!enable) // leveling from on to off
|
||||
if (ubl.state.active) { // leveling from on to off
|
||||
// change unleveled current_position to physical current_position without moving steppers.
|
||||
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
|
||||
else
|
||||
ubl.state.active = false; // disable only AFTER calling apply_leveling
|
||||
}
|
||||
else { // leveling from off to on
|
||||
ubl.state.active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
|
||||
// change physical current_position to unleveled current_position without moving steppers.
|
||||
planner.unapply_leveling(current_position);
|
||||
|
||||
}
|
||||
#else
|
||||
ubl.state.active = enable; // just flip the bit, current_position will be wrong until next move.
|
||||
#endif
|
||||
|
||||
ubl.state.active = enable;
|
||||
|
||||
#else // ABL
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
||||
@ -2384,17 +2395,34 @@ static void clean_up_after_endstop_or_probe_move() {
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
|
||||
void set_z_fade_height(const float zfh) {
|
||||
planner.z_fade_height = zfh;
|
||||
planner.inverse_z_fade_height = RECIPROCAL(zfh);
|
||||
|
||||
if (leveling_is_active())
|
||||
set_current_from_steppers_for_axis(
|
||||
#if ABL_PLANAR
|
||||
ALL_AXES
|
||||
#else
|
||||
Z_AXIS
|
||||
#endif
|
||||
);
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
||||
|
||||
const bool level_active = leveling_is_active();
|
||||
if (level_active) {
|
||||
set_bed_leveling_enabled(false); // turn off before changing fade height for proper apply/unapply leveling to maintain current_position
|
||||
}
|
||||
planner.z_fade_height = zfh;
|
||||
planner.inverse_z_fade_height = RECIPROCAL(zfh);
|
||||
if (level_active) {
|
||||
set_bed_leveling_enabled(true); // turn back on after changing fade height
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
planner.z_fade_height = zfh;
|
||||
planner.inverse_z_fade_height = RECIPROCAL(zfh);
|
||||
|
||||
if (leveling_is_active()) {
|
||||
set_current_from_steppers_for_axis(
|
||||
#if ABL_PLANAR
|
||||
ALL_AXES
|
||||
#else
|
||||
Z_AXIS
|
||||
#endif
|
||||
);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // LEVELING_FADE_HEIGHT
|
||||
@ -3656,6 +3684,7 @@ inline void gcode_G28(const bool always_home_all) {
|
||||
#if ENABLED(DELTA)
|
||||
|
||||
home_delta();
|
||||
UNUSED(always_home_all);
|
||||
|
||||
#else // NOT DELTA
|
||||
|
||||
@ -7592,7 +7621,7 @@ inline void gcode_M92() {
|
||||
/**
|
||||
* Output the current position to serial
|
||||
*/
|
||||
static void report_current_position() {
|
||||
void report_current_position() {
|
||||
SERIAL_PROTOCOLPGM("X:");
|
||||
SERIAL_PROTOCOL(current_position[X_AXIS]);
|
||||
SERIAL_PROTOCOLPGM(" Y:");
|
||||
@ -7611,10 +7640,119 @@ static void report_current_position() {
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef M114_DETAIL
|
||||
|
||||
static const char axis_char[XYZE] = {'X','Y','Z','E'};
|
||||
|
||||
void report_xyze(const float pos[XYZE], uint8_t n = 4, uint8_t precision = 3) {
|
||||
char str[12];
|
||||
for(uint8_t i=0; i<n; i++) {
|
||||
SERIAL_CHAR(' ');
|
||||
SERIAL_CHAR(axis_char[i]);
|
||||
SERIAL_CHAR(':');
|
||||
SERIAL_PROTOCOL(dtostrf(pos[i],8,precision,str));
|
||||
}
|
||||
SERIAL_EOL;
|
||||
}
|
||||
|
||||
inline void report_xyz(const float pos[XYZ]) {
|
||||
report_xyze(pos,3);
|
||||
}
|
||||
|
||||
void report_current_position_detail() {
|
||||
|
||||
stepper.synchronize();
|
||||
|
||||
SERIAL_EOL;
|
||||
SERIAL_PROTOCOLPGM("Logical:");
|
||||
report_xyze(current_position);
|
||||
|
||||
SERIAL_PROTOCOLPGM("Raw: ");
|
||||
const float raw[XYZ] = {
|
||||
RAW_X_POSITION(current_position[X_AXIS]),
|
||||
RAW_Y_POSITION(current_position[Y_AXIS]),
|
||||
RAW_Z_POSITION(current_position[Z_AXIS])
|
||||
};
|
||||
report_xyz(raw);
|
||||
|
||||
SERIAL_PROTOCOLPGM("Leveled:");
|
||||
float leveled[XYZ] = {
|
||||
current_position[X_AXIS],
|
||||
current_position[Y_AXIS],
|
||||
current_position[Z_AXIS]
|
||||
};
|
||||
planner.apply_leveling(leveled);
|
||||
report_xyz(leveled);
|
||||
|
||||
SERIAL_PROTOCOLPGM("UnLevel:");
|
||||
float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] };
|
||||
planner.unapply_leveling(unleveled);
|
||||
report_xyz(unleveled);
|
||||
|
||||
#if IS_KINEMATIC
|
||||
#if IS_SCARA
|
||||
SERIAL_PROTOCOLPGM("ScaraK: ");
|
||||
#else
|
||||
SERIAL_PROTOCOLPGM("DeltaK: ");
|
||||
#endif
|
||||
inverse_kinematics(leveled); // writes delta[]
|
||||
report_xyz(delta);
|
||||
#endif
|
||||
|
||||
SERIAL_PROTOCOLPGM("Stepper:");
|
||||
const float step_count[XYZE] = {
|
||||
(float)stepper.position(X_AXIS),
|
||||
(float)stepper.position(Y_AXIS),
|
||||
(float)stepper.position(Z_AXIS),
|
||||
(float)stepper.position(E_AXIS)
|
||||
};
|
||||
report_xyze(step_count,4,0);
|
||||
|
||||
#if IS_SCARA
|
||||
const float deg[XYZ] = {
|
||||
stepper.get_axis_position_degrees(A_AXIS),
|
||||
stepper.get_axis_position_degrees(B_AXIS)
|
||||
};
|
||||
SERIAL_PROTOCOLPGM("Degrees:");
|
||||
report_xyze(deg,2);
|
||||
#endif
|
||||
|
||||
SERIAL_PROTOCOLPGM("FromStp:");
|
||||
get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics)
|
||||
const float from_steppers[XYZE] = {
|
||||
cartes[X_AXIS],
|
||||
cartes[Y_AXIS],
|
||||
cartes[Z_AXIS],
|
||||
stepper.get_axis_position_mm(E_AXIS)
|
||||
};
|
||||
report_xyze(from_steppers);
|
||||
|
||||
const float diff[XYZE] = {
|
||||
from_steppers[X_AXIS] - leveled[X_AXIS],
|
||||
from_steppers[Y_AXIS] - leveled[Y_AXIS],
|
||||
from_steppers[Z_AXIS] - leveled[Z_AXIS],
|
||||
from_steppers[E_AXIS] - current_position[E_AXIS]
|
||||
};
|
||||
SERIAL_PROTOCOLPGM("Differ: ");
|
||||
report_xyze(diff);
|
||||
}
|
||||
#endif // M114_DETAIL
|
||||
|
||||
/**
|
||||
* M114: Output current position to serial port
|
||||
*/
|
||||
inline void gcode_M114() { stepper.synchronize(); report_current_position(); }
|
||||
inline void gcode_M114() {
|
||||
|
||||
#ifdef M114_DETAIL
|
||||
if ( parser.seen('D') ) {
|
||||
report_current_position_detail();
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
stepper.synchronize();
|
||||
report_current_position();
|
||||
}
|
||||
|
||||
/**
|
||||
* M115: Capabilities string
|
||||
@ -10804,6 +10942,15 @@ void ok_to_send() {
|
||||
/**
|
||||
* Constrain the given coordinates to the software endstops.
|
||||
*/
|
||||
|
||||
// NOTE: This makes no sense for delta beds other than Z-axis.
|
||||
// For delta the X/Y would need to be clamped at
|
||||
// DELTA_PRINTABLE_RADIUS from center of bed, but delta
|
||||
// now enforces is_position_reachable for X/Y regardless
|
||||
// of HAS_SOFTWARE_ENDSTOPS, so that enforcement would be
|
||||
// redundant here. Probably should #ifdef out the X/Y
|
||||
// axis clamps here for delta and just leave the Z clamp.
|
||||
|
||||
void clamp_to_software_endstops(float target[XYZ]) {
|
||||
if (!soft_endstops_enabled) return;
|
||||
#if ENABLED(MIN_SOFTWARE_ENDSTOPS)
|
||||
@ -11597,14 +11744,14 @@ void prepare_move_to_destination() {
|
||||
if (
|
||||
#if IS_KINEMATIC
|
||||
#if UBL_DELTA
|
||||
ubl.prepare_linear_move_to(destination, feedrate_mm_s)
|
||||
ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
|
||||
#else
|
||||
prepare_kinematic_move_to(destination)
|
||||
#endif
|
||||
#elif ENABLED(DUAL_X_CARRIAGE)
|
||||
prepare_move_to_destination_dualx()
|
||||
#elif UBL_DELTA // will work for CARTESIAN too (smaller segments follow mesh more closely)
|
||||
ubl.prepare_linear_move_to(destination, feedrate_mm_s)
|
||||
ubl.prepare_segmented_line_to(destination, feedrate_mm_s)
|
||||
#else
|
||||
prepare_move_to_destination_cartesian()
|
||||
#endif
|
||||
|
||||
@ -535,7 +535,7 @@ void Planner::check_axes_activity() {
|
||||
*/
|
||||
void Planner::apply_leveling(float &lx, float &ly, float &lz) {
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA // probably should also be enabled for UBL without UBL_DELTA
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
||||
if (!ubl.state.active) return;
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
// if z_fade_height enabled (nonzero) and raw_z above it, no leveling required
|
||||
@ -550,7 +550,7 @@ void Planner::check_axes_activity() {
|
||||
if (!abl_enabled) return;
|
||||
#endif
|
||||
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_UBL)
|
||||
static float z_fade_factor = 1.0, last_raw_lz = -999.0;
|
||||
if (z_fade_height) {
|
||||
const float raw_lz = RAW_Z_POSITION(lz);
|
||||
@ -599,36 +599,38 @@ void Planner::check_axes_activity() {
|
||||
|
||||
void Planner::unapply_leveling(float logical[XYZ]) {
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL) && UBL_DELTA
|
||||
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
||||
|
||||
if (ubl.state.active) {
|
||||
|
||||
const float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]),
|
||||
z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
|
||||
float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh;
|
||||
const float z_physical = RAW_Z_POSITION(logical[Z_AXIS]);
|
||||
const float z_ublmesh = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]);
|
||||
const float z_virtual = z_physical - ubl.state.z_offset - z_ublmesh;
|
||||
float z_logical = LOGICAL_Z_POSITION(z_virtual);
|
||||
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
|
||||
// for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height,
|
||||
// Given L==U+O+M(1-U/H) (faded mesh correction formula for U<H)
|
||||
// then U==L-O-M(1-U/H)
|
||||
// so U==L-O-M+MU/H
|
||||
// so U-MU/H==L-O-M
|
||||
// so U(1-M/H)==L-O-M
|
||||
// so U==(L-O-M)/(1-M/H) for U<H
|
||||
// for P=physical_z, L=logical_z, M=mesh_z, O=z_offset, H=fade_height,
|
||||
// Given P=L+O+M(1-L/H) (faded mesh correction formula for L<H)
|
||||
// then L=P-O-M(1-L/H)
|
||||
// so L=P-O-M+ML/H
|
||||
// so L-ML/H=P-O-M
|
||||
// so L(1-M/H)=P-O-M
|
||||
// so L=(P-O-M)/(1-M/H) for L<H
|
||||
|
||||
if (planner.z_fade_height) {
|
||||
const float z_unfaded = z_unlevel / (1.0 - z_ublmesh * planner.inverse_z_fade_height);
|
||||
if (z_unfaded < planner.z_fade_height) // don't know until after compute
|
||||
z_unlevel = z_unfaded;
|
||||
if (z_logical < planner.z_fade_height )
|
||||
z_logical = z_logical / (1.0 - (z_ublmesh * planner.inverse_z_fade_height));
|
||||
if (z_logical >= planner.z_fade_height)
|
||||
z_logical = LOGICAL_Z_POSITION(z_physical - ubl.state.z_offset);
|
||||
}
|
||||
|
||||
#endif // ENABLE_LEVELING_FADE_HEIGHT
|
||||
|
||||
logical[Z_AXIS] = z_unlevel;
|
||||
logical[Z_AXIS] = z_logical;
|
||||
}
|
||||
|
||||
return; // don't fall thru to HAS_ABL or other ENABLE_LEVELING_FADE_HEIGHT logic
|
||||
return; // don't fall thru to other ENABLE_LEVELING_FADE_HEIGHT logic
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -83,7 +83,7 @@
|
||||
}
|
||||
|
||||
void unified_bed_leveling::reset() {
|
||||
state.active = false;
|
||||
set_bed_leveling_enabled(false);
|
||||
state.z_offset = 0;
|
||||
state.storage_slot = -1;
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
@ -94,11 +94,17 @@
|
||||
}
|
||||
|
||||
void unified_bed_leveling::invalidate() {
|
||||
state.active = false;
|
||||
set_bed_leveling_enabled(false);
|
||||
state.z_offset = 0;
|
||||
for (int x = 0; x < GRID_MAX_POINTS_X; x++)
|
||||
for (int y = 0; y < GRID_MAX_POINTS_Y; y++)
|
||||
z_values[x][y] = NAN;
|
||||
set_all_mesh_points_to_value(NAN);
|
||||
}
|
||||
|
||||
void unified_bed_leveling::set_all_mesh_points_to_value(float value) {
|
||||
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
|
||||
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
|
||||
z_values[x][y] = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void unified_bed_leveling::display_map(const int map_type) {
|
||||
|
||||
@ -154,6 +154,7 @@
|
||||
static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
|
||||
static void reset();
|
||||
static void invalidate();
|
||||
static void set_all_mesh_points_to_value(float);
|
||||
static bool sanity_check();
|
||||
|
||||
static void G29() _O0; // O0 for no optimization
|
||||
@ -385,7 +386,7 @@
|
||||
FORCE_INLINE static float mesh_index_to_xpos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_xpos[i]); }
|
||||
FORCE_INLINE static float mesh_index_to_ypos(const uint8_t i) { return pgm_read_float(&_mesh_index_to_ypos[i]); }
|
||||
|
||||
static bool prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate);
|
||||
static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate);
|
||||
static void line_to_destination_cartesian(const float &fr, uint8_t e);
|
||||
|
||||
}; // class unified_bed_leveling
|
||||
|
||||
@ -30,6 +30,7 @@
|
||||
#include "configuration_store.h"
|
||||
#include "ultralcd.h"
|
||||
#include "stepper.h"
|
||||
#include "planner.h"
|
||||
#include "gcode.h"
|
||||
|
||||
#include <math.h>
|
||||
@ -48,6 +49,7 @@
|
||||
extern long babysteps_done;
|
||||
extern float probe_pt(const float &x, const float &y, bool, int);
|
||||
extern bool set_probe_deployed(bool);
|
||||
extern void set_bed_leveling_enabled(bool);
|
||||
|
||||
#define SIZE_OF_LITTLE_RAISE 1
|
||||
#define BIG_RAISE_NOT_NEEDED 0
|
||||
@ -325,15 +327,23 @@
|
||||
if (parser.seen('I')) {
|
||||
uint8_t cnt = 0;
|
||||
g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1;
|
||||
while (g29_repetition_cnt--) {
|
||||
if (cnt > 20) { cnt = 0; idle(); }
|
||||
const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
|
||||
if (location.x_index < 0) {
|
||||
SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
|
||||
break; // No more invalid Mesh Points to populate
|
||||
if (g29_repetition_cnt >= GRID_MAX_POINTS) {
|
||||
set_all_mesh_points_to_value(NAN);
|
||||
} else {
|
||||
while (g29_repetition_cnt--) {
|
||||
if (cnt > 20) { cnt = 0; idle(); }
|
||||
const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
|
||||
if (location.x_index < 0) {
|
||||
// No more REACHABLE mesh points to invalidate, so we ASSUME the user
|
||||
// meant to invalidate the ENTIRE mesh, which cannot be done with
|
||||
// find_closest_mesh_point loop which only returns REACHABLE points.
|
||||
set_all_mesh_points_to_value(NAN);
|
||||
SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
|
||||
break; // No more invalid Mesh Points to populate
|
||||
}
|
||||
z_values[location.x_index][location.y_index] = NAN;
|
||||
cnt++;
|
||||
}
|
||||
z_values[location.x_index][location.y_index] = NAN;
|
||||
cnt++;
|
||||
}
|
||||
SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
|
||||
}
|
||||
@ -497,18 +507,26 @@
|
||||
* - Specify a constant with the 'C' parameter.
|
||||
* - Allow 'G29 P3' to choose a 'reasonable' constant.
|
||||
*/
|
||||
|
||||
if (g29_c_flag) {
|
||||
if (g29_repetition_cnt >= GRID_MAX_POINTS) {
|
||||
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
|
||||
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
|
||||
z_values[x][y] = g29_constant;
|
||||
}
|
||||
}
|
||||
set_all_mesh_points_to_value(g29_constant);
|
||||
}
|
||||
else {
|
||||
while (g29_repetition_cnt--) { // this only populates reachable mesh points near
|
||||
const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
|
||||
if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
|
||||
if (location.x_index < 0) {
|
||||
// No more REACHABLE INVALID mesh points to populate, so we ASSUME
|
||||
// user meant to populate ALL INVALID mesh points to value
|
||||
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
|
||||
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
|
||||
if ( isnan(z_values[x][y])) {
|
||||
z_values[x][y] = g29_constant;
|
||||
}
|
||||
}
|
||||
}
|
||||
break; // No more invalid Mesh Points to populate
|
||||
}
|
||||
z_values[location.x_index][location.y_index] = g29_constant;
|
||||
}
|
||||
}
|
||||
@ -999,12 +1017,15 @@
|
||||
|
||||
serialprintPGM(parser.seen('B') ? PSTR("Place shim & measure") : PSTR("Measure")); // TODO: Make translatable strings
|
||||
|
||||
const float z_step = 0.01; // existing behavior: 0.01mm per click, occasionally step
|
||||
//const float z_step = 1.0 / planner.axis_steps_per_mm[Z_AXIS]; // approx one step each click
|
||||
|
||||
while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel
|
||||
delay(50); // debounce
|
||||
while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here!
|
||||
idle();
|
||||
if (encoder_diff) {
|
||||
do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) / 100.0);
|
||||
do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * z_step);
|
||||
encoder_diff = 0;
|
||||
}
|
||||
}
|
||||
@ -1115,11 +1136,11 @@
|
||||
SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
|
||||
return UBL_ERR;
|
||||
}
|
||||
state.active = true;
|
||||
set_bed_leveling_enabled(true);
|
||||
report_state();
|
||||
}
|
||||
else if (parser.seen('D')) {
|
||||
state.active = false;
|
||||
set_bed_leveling_enabled(false);
|
||||
report_state();
|
||||
}
|
||||
|
||||
@ -1158,7 +1179,7 @@
|
||||
return;
|
||||
}
|
||||
ubl_state_at_invocation = state.active;
|
||||
state.active = 0;
|
||||
set_bed_leveling_enabled(false);
|
||||
}
|
||||
|
||||
void unified_bed_leveling::restore_ubl_active_state_and_leave() {
|
||||
@ -1168,7 +1189,7 @@
|
||||
lcd_quick_feedback();
|
||||
return;
|
||||
}
|
||||
state.active = ubl_state_at_invocation;
|
||||
set_bed_leveling_enabled(ubl_state_at_invocation);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -1695,6 +1716,8 @@
|
||||
SERIAL_EOL;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (do_ubl_mesh_map) display_map(g29_map_type);
|
||||
}
|
||||
|
||||
#if ENABLED(UBL_G29_P31)
|
||||
|
||||
@ -32,7 +32,25 @@
|
||||
|
||||
extern float destination[XYZE];
|
||||
extern void set_current_to_destination();
|
||||
extern float delta_segments_per_second;
|
||||
|
||||
#if ENABLED(DELTA)
|
||||
|
||||
extern float delta[ABC],
|
||||
endstop_adj[ABC];
|
||||
|
||||
extern float delta_radius,
|
||||
delta_tower_angle_trim[2],
|
||||
delta_tower[ABC][2],
|
||||
delta_diagonal_rod,
|
||||
delta_calibration_radius,
|
||||
delta_diagonal_rod_2_tower[ABC],
|
||||
delta_segments_per_second,
|
||||
delta_clip_start_height;
|
||||
|
||||
extern float delta_safe_distance_from_top();
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
static void debug_echo_axis(const AxisEnum axis) {
|
||||
if (current_position[axis] == destination[axis])
|
||||
@ -470,51 +488,76 @@
|
||||
#endif
|
||||
|
||||
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
|
||||
// so we call _buffer_line directly here. Per-segmented leveling performed first.
|
||||
// so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
|
||||
|
||||
static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
|
||||
inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) {
|
||||
|
||||
#if IS_KINEMATIC
|
||||
#if ENABLED(DELTA) // apply delta inverse_kinematics
|
||||
|
||||
inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget
|
||||
float feedrate = fr_mm_s;
|
||||
const float delta_A = rz + sqrt( delta_diagonal_rod_2_tower[A_AXIS]
|
||||
- HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
|
||||
delta_tower[A_AXIS][Y_AXIS] - ry ));
|
||||
|
||||
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
|
||||
float adiff = abs(delta[A_AXIS] - scara_oldA),
|
||||
bdiff = abs(delta[B_AXIS] - scara_oldB);
|
||||
scara_oldA = delta[A_AXIS];
|
||||
scara_oldB = delta[B_AXIS];
|
||||
feedrate = max(adiff, bdiff) * scara_feed_factor;
|
||||
#endif
|
||||
const float delta_B = rz + sqrt( delta_diagonal_rod_2_tower[B_AXIS]
|
||||
- HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
|
||||
delta_tower[B_AXIS][Y_AXIS] - ry ));
|
||||
|
||||
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
|
||||
const float delta_C = rz + sqrt( delta_diagonal_rod_2_tower[C_AXIS]
|
||||
- HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
|
||||
delta_tower[C_AXIS][Y_AXIS] - ry ));
|
||||
|
||||
#else // cartesian
|
||||
planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_extruder);
|
||||
|
||||
planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder);
|
||||
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
|
||||
|
||||
const float lseg[XYZ] = { LOGICAL_X_POSITION(rx),
|
||||
LOGICAL_Y_POSITION(ry),
|
||||
LOGICAL_Z_POSITION(rz)
|
||||
};
|
||||
|
||||
inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
|
||||
// should move the feedrate scaling to scara inverse_kinematics
|
||||
|
||||
float adiff = abs(delta[A_AXIS] - scara_oldA),
|
||||
bdiff = abs(delta[B_AXIS] - scara_oldB);
|
||||
scara_oldA = delta[A_AXIS];
|
||||
scara_oldB = delta[B_AXIS];
|
||||
float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
|
||||
|
||||
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder);
|
||||
|
||||
#else // CARTESIAN
|
||||
|
||||
// Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates
|
||||
|
||||
const float lx = LOGICAL_X_POSITION(rx),
|
||||
ly = LOGICAL_Y_POSITION(ry),
|
||||
lz = LOGICAL_Z_POSITION(rz);
|
||||
|
||||
planner._buffer_line(lx, ly, lz, le, fr, active_extruder);
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
|
||||
* Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
|
||||
* This calls planner._buffer_line multiple times for small incremental moves.
|
||||
* Returns true if the caller did NOT update current_position, otherwise false.
|
||||
* Returns true if did NOT move, false if moved (requires current_position update).
|
||||
*/
|
||||
|
||||
static bool unified_bed_leveling::prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) {
|
||||
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) {
|
||||
|
||||
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) // fail if moving outside reachable boundary
|
||||
return true; // did not move, so current_position still accurate
|
||||
|
||||
const float difference[XYZE] = { // cartesian distances moved in XYZE
|
||||
ltarget[X_AXIS] - current_position[X_AXIS],
|
||||
ltarget[Y_AXIS] - current_position[Y_AXIS],
|
||||
ltarget[Z_AXIS] - current_position[Z_AXIS],
|
||||
ltarget[E_AXIS] - current_position[E_AXIS]
|
||||
};
|
||||
const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS],
|
||||
tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS],
|
||||
tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS],
|
||||
tot_de = ltarget[E_AXIS] - current_position[E_AXIS];
|
||||
|
||||
const float cartesian_xy_mm = HYPOT(difference[X_AXIS], difference[Y_AXIS]); // total horizontal xy distance
|
||||
const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance
|
||||
|
||||
#if IS_KINEMATIC
|
||||
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
|
||||
@ -534,16 +577,19 @@
|
||||
scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
|
||||
#endif
|
||||
|
||||
const float segment_distance[XYZE] = { // length for each segment
|
||||
difference[X_AXIS] * inv_segments,
|
||||
difference[Y_AXIS] * inv_segments,
|
||||
difference[Z_AXIS] * inv_segments,
|
||||
difference[E_AXIS] * inv_segments
|
||||
};
|
||||
const float seg_dx = tot_dx * inv_segments,
|
||||
seg_dy = tot_dy * inv_segments,
|
||||
seg_dz = tot_dz * inv_segments,
|
||||
seg_de = tot_de * inv_segments;
|
||||
|
||||
// Note that E segment distance could vary slightly as z mesh height
|
||||
// changes for each segment, but small enough to ignore.
|
||||
|
||||
float seg_rx = RAW_X_POSITION(current_position[X_AXIS]),
|
||||
seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]),
|
||||
seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]),
|
||||
seg_le = current_position[E_AXIS];
|
||||
|
||||
const bool above_fade_height = (
|
||||
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
||||
planner.z_fade_height != 0 && planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS])
|
||||
@ -558,21 +604,24 @@
|
||||
|
||||
const float z_offset = state.active ? state.z_offset : 0.0;
|
||||
|
||||
float seg_dest[XYZE]; // per-segment destination,
|
||||
COPY_XYZE(seg_dest, current_position); // starting from current position
|
||||
do {
|
||||
|
||||
while (--segments) {
|
||||
LOOP_XYZE(i) seg_dest[i] += segment_distance[i];
|
||||
float ztemp = seg_dest[Z_AXIS];
|
||||
seg_dest[Z_AXIS] += z_offset;
|
||||
ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
|
||||
seg_dest[Z_AXIS] = ztemp;
|
||||
}
|
||||
if (--segments) { // not the last segment
|
||||
seg_rx += seg_dx;
|
||||
seg_ry += seg_dy;
|
||||
seg_rz += seg_dz;
|
||||
seg_le += seg_de;
|
||||
} else { // last segment, use exact destination
|
||||
seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
|
||||
seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
|
||||
seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
|
||||
seg_le = ltarget[E_AXIS];
|
||||
}
|
||||
|
||||
ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_offset, seg_le, feedrate );
|
||||
|
||||
} while (segments);
|
||||
|
||||
// Since repeated adding segment_distance accumulates small errors, final move to exact destination.
|
||||
COPY_XYZE(seg_dest, ltarget);
|
||||
seg_dest[Z_AXIS] += z_offset;
|
||||
ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
|
||||
return false; // moved but did not set_current_to_destination();
|
||||
}
|
||||
|
||||
@ -582,14 +631,11 @@
|
||||
const float fade_scaling_factor = fade_scaling_factor_for_z(ltarget[Z_AXIS]);
|
||||
#endif
|
||||
|
||||
float seg_dest[XYZE]; // per-segment destination, initialize to first segment
|
||||
LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i];
|
||||
|
||||
const float &dx_seg = segment_distance[X_AXIS]; // alias for clarity
|
||||
const float &dy_seg = segment_distance[Y_AXIS];
|
||||
|
||||
float rx = RAW_X_POSITION(seg_dest[X_AXIS]), // assume raw vs logical coordinates shifted but not scaled.
|
||||
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
|
||||
// increment to first segment destination
|
||||
seg_rx += seg_dx;
|
||||
seg_ry += seg_dy;
|
||||
seg_rz += seg_dz;
|
||||
seg_le += seg_de;
|
||||
|
||||
for(;;) { // for each mesh cell encountered during the move
|
||||
|
||||
@ -600,20 +646,16 @@
|
||||
// in top of loop and again re-find same adjacent cell and use it, just less efficient
|
||||
// for mesh inset area.
|
||||
|
||||
int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
||||
cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
||||
int8_t cell_xi = (seg_rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
||||
cell_yi = (seg_ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
||||
|
||||
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
|
||||
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
|
||||
|
||||
const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
|
||||
y0 = mesh_index_to_ypos(cell_yi), // 64 byte table lookup avoids mul+add
|
||||
x1 = mesh_index_to_xpos(cell_xi + 1), // 64 byte table lookup avoids mul+add
|
||||
y1 = mesh_index_to_ypos(cell_yi + 1); // 64 byte table lookup avoids mul+add
|
||||
const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
|
||||
y0 = mesh_index_to_ypos(cell_yi);
|
||||
|
||||
float cx = rx - x0, // cell-relative x
|
||||
cy = ry - y0, // cell-relative y
|
||||
z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
|
||||
float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
|
||||
z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
|
||||
z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
|
||||
z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
|
||||
@ -623,15 +665,18 @@
|
||||
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
|
||||
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
|
||||
|
||||
float cx = seg_rx - x0, // cell-relative x and y
|
||||
cy = seg_ry - y0;
|
||||
|
||||
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
|
||||
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
|
||||
|
||||
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx
|
||||
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
|
||||
|
||||
const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
|
||||
z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
|
||||
|
||||
float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1
|
||||
float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
|
||||
|
||||
// float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
|
||||
|
||||
@ -639,8 +684,8 @@
|
||||
// and the z_cxym slope will change, both as a function of cx within the cell, and
|
||||
// each change by a constant for fixed segment lengths.
|
||||
|
||||
const float z_sxy0 = z_xmy0 * dx_seg, // per-segment adjustment to z_cxy0
|
||||
z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * dx_seg; // per-segment adjustment to z_cxym
|
||||
const float z_sxy0 = z_xmy0 * seg_dx, // per-segment adjustment to z_cxy0
|
||||
z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx; // per-segment adjustment to z_cxym
|
||||
|
||||
for(;;) { // for all segments within this mesh cell
|
||||
|
||||
@ -650,28 +695,29 @@
|
||||
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
|
||||
#endif
|
||||
|
||||
z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z
|
||||
z_cxcy += state.z_offset; // add fixed mesh offset from G29 Z
|
||||
|
||||
if (--segments == 0) { // if this is last segment, use ltarget for exact
|
||||
COPY_XYZE(seg_dest, ltarget);
|
||||
seg_dest[Z_AXIS] += z_cxcy;
|
||||
ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
|
||||
return false; // did not set_current_to_destination()
|
||||
seg_rx = RAW_X_POSITION(ltarget[X_AXIS]);
|
||||
seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]);
|
||||
seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]);
|
||||
seg_le = ltarget[E_AXIS];
|
||||
}
|
||||
|
||||
const float z_orig = seg_dest[Z_AXIS]; // remember the pre-leveled segment z value
|
||||
seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling
|
||||
ubl_buffer_line_segment(seg_dest, feedrate, active_extruder);
|
||||
seg_dest[Z_AXIS] = z_orig; // restore pre-leveled z before incrementing
|
||||
ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate );
|
||||
|
||||
LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment
|
||||
if (segments == 0 ) // done with last segment
|
||||
return false; // did not set_current_to_destination()
|
||||
|
||||
cx += dx_seg;
|
||||
cy += dy_seg;
|
||||
seg_rx += seg_dx;
|
||||
seg_ry += seg_dy;
|
||||
seg_rz += seg_dz;
|
||||
seg_le += seg_de;
|
||||
|
||||
cx += seg_dx;
|
||||
cy += seg_dy;
|
||||
|
||||
if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) { // done within this cell, break to next
|
||||
rx = RAW_X_POSITION(seg_dest[X_AXIS]);
|
||||
ry = RAW_Y_POSITION(seg_dest[Y_AXIS]);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user