andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
pull/1/head
oldmcg 8 years ago
committed by Roxy-3D
parent
862def2ef4
commit
9f295581f0
7 changed files with 380 additions and 156 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      Marlin/G26_Mesh_Validation_Tool.cpp
  2. 207
      Marlin/Marlin_main.cpp
  3. 38
      Marlin/planner.cpp
  4. 16
      Marlin/ubl.cpp
  5. 3
      Marlin/ubl.h
  6. 61
      Marlin/ubl_G29.cpp
  7. 206
      Marlin/ubl_motion.cpp

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 ));
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 ));
planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_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
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder);
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;
#else // cartesian
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], le, s_feedrate, active_extruder);
planner._buffer_line(ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, 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 {
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];
}
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;
}
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 );
if (segments == 0 ) // done with last segment
return false; // did not set_current_to_destination()
LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment
seg_rx += seg_dx;
seg_ry += seg_dy;
seg_rz += seg_dz;
seg_le += seg_de;
cx += dx_seg;
cy += dy_seg;
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;
}

Write Preview
Loading…
Cancel
Save