MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Ubl delta fixes and improvements (#6941)

Browse Source 
* 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 2017-06-03 17:11:43 -05:00 committed by Roxy-3D
parent 862def2ef4
commit 9f295581f0
7 changed files with 380 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Marlin/G26_Mesh_Validation_Tool.cpp
View File

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

207
Marlin/Marlin_main.cpp
View File

@ -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
planner.unapply_leveling(current_position);
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

38
Marlin/planner.cpp
View File

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

16
Marlin/ubl.cpp
View File

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

3
Marlin/ubl.h
View File

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

61
Marlin/ubl_G29.cpp
View File

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

206
Marlin/ubl_motion.cpp
View File

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