SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

Operate in Native Machine Space

Browse source
This commit is contained in:
Scott Lahteine 2017-11-02 23:59:42 -05:00
parent 31f112cf58
commit f8393a0908
36 changed files with 449 additions and 489 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
Marlin/src/core/utility.cpp
View file

@ -365,10 +365,10 @@ void safe_delay(millis_t ms) {
SERIAL_ECHOPGM("Mesh Bed Leveling"); SERIAL_ECHOPGM("Mesh Bed Leveling");
if (planner.leveling_active) { if (planner.leveling_active) {
float lz = current_position[Z_AXIS]; float rz = current_position[Z_AXIS];
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], lz); planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], rz);
SERIAL_ECHOLNPGM(" (enabled)"); SERIAL_ECHOLNPGM(" (enabled)");
SERIAL_ECHOPAIR("MBL Adjustment Z", lz); SERIAL_ECHOPAIR("MBL Adjustment Z", rz);
} }
else else
SERIAL_ECHOPGM(" (disabled)"); SERIAL_ECHOPGM(" (disabled)");

30
Marlin/src/feature/bedlevel/abl/abl.cpp
View file

@ -259,7 +259,7 @@ void refresh_bed_level() {
#endif #endif
// Get the Z adjustment for non-linear bed leveling // Get the Z adjustment for non-linear bed leveling
float bilinear_z_offset(const float logical[XYZ]) { float bilinear_z_offset(const float raw[XYZ]) {
static float z1, d2, z3, d4, L, D, ratio_x, ratio_y, static float z1, d2, z3, d4, L, D, ratio_x, ratio_y,
last_x = -999.999, last_y = -999.999; last_x = -999.999, last_y = -999.999;
@ -269,8 +269,8 @@ float bilinear_z_offset(const float logical[XYZ]) {
last_gridx = -99, last_gridy = -99; last_gridx = -99, last_gridy = -99;
// XY relative to the probed area // XY relative to the probed area
const float x = RAW_X_POSITION(logical[X_AXIS]) - bilinear_start[X_AXIS], const float rx = raw[X_AXIS] - bilinear_start[X_AXIS],
y = RAW_Y_POSITION(logical[Y_AXIS]) - bilinear_start[Y_AXIS]; ry = raw[Y_AXIS] - bilinear_start[Y_AXIS];
#if ENABLED(EXTRAPOLATE_BEYOND_GRID) #if ENABLED(EXTRAPOLATE_BEYOND_GRID)
// Keep using the last grid box // Keep using the last grid box
@ -280,9 +280,9 @@ float bilinear_z_offset(const float logical[XYZ]) {
#define FAR_EDGE_OR_BOX 1 #define FAR_EDGE_OR_BOX 1
#endif #endif
if (last_x != x) { if (last_x != rx) {
last_x = x; last_x = rx;
ratio_x = x * ABL_BG_FACTOR(X_AXIS); ratio_x = rx * ABL_BG_FACTOR(X_AXIS);
const float gx = constrain(FLOOR(ratio_x), 0, ABL_BG_POINTS_X - FAR_EDGE_OR_BOX); const float gx = constrain(FLOOR(ratio_x), 0, ABL_BG_POINTS_X - FAR_EDGE_OR_BOX);
ratio_x -= gx; // Subtract whole to get the ratio within the grid box ratio_x -= gx; // Subtract whole to get the ratio within the grid box
@ -295,11 +295,11 @@ float bilinear_z_offset(const float logical[XYZ]) {
nextx = min(gridx + 1, ABL_BG_POINTS_X - 1); nextx = min(gridx + 1, ABL_BG_POINTS_X - 1);
} }
if (last_y != y || last_gridx != gridx) { if (last_y != ry || last_gridx != gridx) {
if (last_y != y) { if (last_y != ry) {
last_y = y; last_y = ry;
ratio_y = y * ABL_BG_FACTOR(Y_AXIS); ratio_y = ry * ABL_BG_FACTOR(Y_AXIS);
const float gy = constrain(FLOOR(ratio_y), 0, ABL_BG_POINTS_Y - FAR_EDGE_OR_BOX); const float gy = constrain(FLOOR(ratio_y), 0, ABL_BG_POINTS_Y - FAR_EDGE_OR_BOX);
ratio_y -= gy; ratio_y -= gy;
@ -322,7 +322,7 @@ float bilinear_z_offset(const float logical[XYZ]) {
d4 = ABL_BG_GRID(nextx, nexty) - z3; // right-back (delta) d4 = ABL_BG_GRID(nextx, nexty) - z3; // right-back (delta)
} }
// Bilinear interpolate. Needed since y or gridx has changed. // Bilinear interpolate. Needed since ry or gridx has changed.
L = z1 + d2 * ratio_y; // Linear interp. LF -> LB L = z1 + d2 * ratio_y; // Linear interp. LF -> LB
const float R = z3 + d4 * ratio_y; // Linear interp. RF -> RB const float R = z3 + d4 * ratio_y; // Linear interp. RF -> RB
@ -335,10 +335,10 @@ float bilinear_z_offset(const float logical[XYZ]) {
static float last_offset = 0; static float last_offset = 0;
if (FABS(last_offset - offset) > 0.2) { if (FABS(last_offset - offset) > 0.2) {
SERIAL_ECHOPGM("Sudden Shift at "); SERIAL_ECHOPGM("Sudden Shift at ");
SERIAL_ECHOPAIR("x=", x); SERIAL_ECHOPAIR("x=", rx);
SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[X_AXIS]); SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[X_AXIS]);
SERIAL_ECHOLNPAIR(" -> gridx=", gridx); SERIAL_ECHOLNPAIR(" -> gridx=", gridx);
SERIAL_ECHOPAIR(" y=", y); SERIAL_ECHOPAIR(" y=", ry);
SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[Y_AXIS]); SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[Y_AXIS]);
SERIAL_ECHOLNPAIR(" -> gridy=", gridy); SERIAL_ECHOLNPAIR(" -> gridy=", gridy);
SERIAL_ECHOPAIR(" ratio_x=", ratio_x); SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
@ -390,14 +390,14 @@ float bilinear_z_offset(const float logical[XYZ]) {
const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
if (cx2 != cx1 && TEST(x_splits, gcx)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = LOGICAL_X_POSITION(bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx); destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx;
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = LINE_SEGMENT_END(Y); destination[Y_AXIS] = LINE_SEGMENT_END(Y);
CBI(x_splits, gcx); CBI(x_splits, gcx);
} }
else if (cy2 != cy1 && TEST(y_splits, gcy)) { else if (cy2 != cy1 && TEST(y_splits, gcy)) {
COPY(end, destination); COPY(end, destination);
destination[Y_AXIS] = LOGICAL_Y_POSITION(bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy); destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy;
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = LINE_SEGMENT_END(X); destination[X_AXIS] = LINE_SEGMENT_END(X);
CBI(y_splits, gcy); CBI(y_splits, gcy);

2
Marlin/src/feature/bedlevel/abl/abl.h
View file

@ -32,7 +32,7 @@
extern int bilinear_grid_spacing[2], bilinear_start[2]; extern int bilinear_grid_spacing[2], bilinear_start[2];
extern float bilinear_grid_factor[2], extern float bilinear_grid_factor[2],
z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
float bilinear_z_offset(const float logical[XYZ]); float bilinear_z_offset(const float raw[XYZ]);
void extrapolate_unprobed_bed_level(); void extrapolate_unprobed_bed_level();
void print_bilinear_leveling_grid(); void print_bilinear_leveling_grid();

8
Marlin/src/feature/bedlevel/bedlevel.cpp
View file

@ -256,18 +256,18 @@ void reset_bed_level() {
#if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY) #if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY)
void _manual_goto_xy(const float &x, const float &y) { void _manual_goto_xy(const float &rx, const float &ry) {
const float old_feedrate_mm_s = feedrate_mm_s; const float old_feedrate_mm_s = feedrate_mm_s;
#if MANUAL_PROBE_HEIGHT > 0 #if MANUAL_PROBE_HEIGHT > 0
const float prev_z = current_position[Z_AXIS]; const float prev_z = current_position[Z_AXIS];
feedrate_mm_s = homing_feedrate(Z_AXIS); feedrate_mm_s = homing_feedrate(Z_AXIS);
current_position[Z_AXIS] = LOGICAL_Z_POSITION(MANUAL_PROBE_HEIGHT); current_position[Z_AXIS] = MANUAL_PROBE_HEIGHT;
line_to_current_position(); line_to_current_position();
#endif #endif
feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED); feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED);
current_position[X_AXIS] = LOGICAL_X_POSITION(x); current_position[X_AXIS] = rx;
current_position[Y_AXIS] = LOGICAL_Y_POSITION(y); current_position[Y_AXIS] = ry;
line_to_current_position(); line_to_current_position();
#if MANUAL_PROBE_HEIGHT > 0 #if MANUAL_PROBE_HEIGHT > 0

12
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
View file

@ -57,10 +57,10 @@
* splitting the move where it crosses mesh borders. * splitting the move where it crosses mesh borders.
*/ */
void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) { void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
int cx1 = mbl.cell_index_x(RAW_CURRENT_POSITION(X)), int cx1 = mbl.cell_index_x(current_position[X_AXIS]),
cy1 = mbl.cell_index_y(RAW_CURRENT_POSITION(Y)), cy1 = mbl.cell_index_y(current_position[Y_AXIS]),
cx2 = mbl.cell_index_x(RAW_X_POSITION(destination[X_AXIS])), cx2 = mbl.cell_index_x(destination[X_AXIS]),
cy2 = mbl.cell_index_y(RAW_Y_POSITION(destination[Y_AXIS])); cy2 = mbl.cell_index_y(destination[Y_AXIS]);
NOMORE(cx1, GRID_MAX_POINTS_X - 2); NOMORE(cx1, GRID_MAX_POINTS_X - 2);
NOMORE(cy1, GRID_MAX_POINTS_Y - 2); NOMORE(cy1, GRID_MAX_POINTS_Y - 2);
NOMORE(cx2, GRID_MAX_POINTS_X - 2); NOMORE(cx2, GRID_MAX_POINTS_X - 2);
@ -81,14 +81,14 @@
const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2);
if (cx2 != cx1 && TEST(x_splits, gcx)) { if (cx2 != cx1 && TEST(x_splits, gcx)) {
COPY(end, destination); COPY(end, destination);
destination[X_AXIS] = LOGICAL_X_POSITION(mbl.index_to_xpos[gcx]); destination[X_AXIS] = mbl.index_to_xpos[gcx];
normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]);
destination[Y_AXIS] = MBL_SEGMENT_END(Y); destination[Y_AXIS] = MBL_SEGMENT_END(Y);
CBI(x_splits, gcx); CBI(x_splits, gcx);
} }
else if (cy2 != cy1 && TEST(y_splits, gcy)) { else if (cy2 != cy1 && TEST(y_splits, gcy)) {
COPY(end, destination); COPY(end, destination);
destination[Y_AXIS] = LOGICAL_Y_POSITION(mbl.index_to_ypos[gcy]); destination[Y_AXIS] = mbl.index_to_ypos[gcy];
normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]);
destination[X_AXIS] = MBL_SEGMENT_END(X); destination[X_AXIS] = MBL_SEGMENT_END(X);
CBI(y_splits, gcy); CBI(y_splits, gcy);

36
Marlin/src/feature/bedlevel/ubl/G26_Mesh_Validation_Tool.cpp
View file

@ -276,7 +276,7 @@ void unified_bed_leveling::G26() {
// If this mesh location is outside the printable_radius, skip it. // If this mesh location is outside the printable_radius, skip it.
if (!position_is_reachable_raw_xy(circle_x, circle_y)) continue; if (!position_is_reachable(circle_x, circle_y)) continue;
xi = location.x_index; // Just to shrink the next few lines and make them easier to understand xi = location.x_index; // Just to shrink the next few lines and make them easier to understand
yi = location.y_index; yi = location.y_index;
@ -325,16 +325,16 @@ void unified_bed_leveling::G26() {
if (tmp_div_30 < 0) tmp_div_30 += 360 / 30; if (tmp_div_30 < 0) tmp_div_30 += 360 / 30;
if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30; if (tmp_div_30 > 11) tmp_div_30 -= 360 / 30;
float x = circle_x + cos_table[tmp_div_30], // for speed, these are now a lookup table entry float rx = circle_x + cos_table[tmp_div_30], // for speed, these are now a lookup table entry
y = circle_y + sin_table[tmp_div_30], ry = circle_y + sin_table[tmp_div_30],
xe = circle_x + cos_table[tmp_div_30 + 1], xe = circle_x + cos_table[tmp_div_30 + 1],
ye = circle_y + sin_table[tmp_div_30 + 1]; ye = circle_y + sin_table[tmp_div_30 + 1];
#if IS_KINEMATIC #if IS_KINEMATIC
// Check to make sure this segment is entirely on the bed, skip if not. // Check to make sure this segment is entirely on the bed, skip if not.
if (!position_is_reachable_raw_xy(x, y) || !position_is_reachable_raw_xy(xe, ye)) continue; if (!position_is_reachable(rx, ry) || !position_is_reachable(xe, ye)) continue;
#else // not, we need to skip #else // not, we need to skip
x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops rx = constrain(rx, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops
y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1); ry = constrain(ry, Y_MIN_POS + 1, Y_MAX_POS - 1);
xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1); xe = constrain(xe, X_MIN_POS + 1, X_MAX_POS - 1);
ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1); ye = constrain(ye, Y_MIN_POS + 1, Y_MAX_POS - 1);
#endif #endif
@ -350,7 +350,7 @@ void unified_bed_leveling::G26() {
// debug_current_and_destination(seg_msg); // debug_current_and_destination(seg_msg);
//} //}
print_line_from_here_to_there(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), g26_layer_height, LOGICAL_X_POSITION(xe), LOGICAL_Y_POSITION(ye), g26_layer_height); print_line_from_here_to_there(rx, ry, g26_layer_height, xe, ye, g26_layer_height);
} }
if (look_for_lines_to_connect()) if (look_for_lines_to_connect())
@ -456,7 +456,7 @@ bool unified_bed_leveling::look_for_lines_to_connect() {
sy = ey = constrain(mesh_index_to_ypos(j), Y_MIN_POS + 1, Y_MAX_POS - 1); sy = ey = constrain(mesh_index_to_ypos(j), Y_MIN_POS + 1, Y_MAX_POS - 1);
ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1);
if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) { if (position_is_reachable(sx, sy) && position_is_reachable(ex, ey)) {
if (g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx);
@ -468,7 +468,7 @@ bool unified_bed_leveling::look_for_lines_to_connect() {
//debug_current_and_destination(PSTR("Connecting horizontal line.")); //debug_current_and_destination(PSTR("Connecting horizontal line."));
} }
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), g26_layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), g26_layer_height); print_line_from_here_to_there(sx, sy, g26_layer_height, ex, ey, g26_layer_height);
} }
bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it
} }
@ -490,7 +490,7 @@ bool unified_bed_leveling::look_for_lines_to_connect() {
sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1);
ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1);
if (position_is_reachable_raw_xy(sx, sy) && position_is_reachable_raw_xy(ex, ey)) { if (position_is_reachable(sx, sy) && position_is_reachable(ex, ey)) {
if (g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx);
@ -501,7 +501,7 @@ bool unified_bed_leveling::look_for_lines_to_connect() {
SERIAL_EOL(); SERIAL_EOL();
debug_current_and_destination(PSTR("Connecting vertical line.")); debug_current_and_destination(PSTR("Connecting vertical line."));
} }
print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), g26_layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), g26_layer_height); print_line_from_here_to_there(sx, sy, g26_layer_height, ex, ey, g26_layer_height);
} }
bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped
} }
@ -513,11 +513,11 @@ bool unified_bed_leveling::look_for_lines_to_connect() {
return false; return false;
} }
void unified_bed_leveling::move_to(const float &x, const float &y, const float &z, const float &e_delta) { void unified_bed_leveling::move_to(const float &rx, const float &ry, const float &z, const float &e_delta) {
float feed_value; float feed_value;
static float last_z = -999.99; static float last_z = -999.99;
bool has_xy_component = (x != current_position[X_AXIS] || y != current_position[Y_AXIS]); // Check if X or Y is involved in the movement. bool has_xy_component = (rx != current_position[X_AXIS] || ry != current_position[Y_AXIS]); // Check if X or Y is involved in the movement.
if (z != last_z) { if (z != last_z) {
last_z = z; last_z = z;
@ -540,8 +540,8 @@ void unified_bed_leveling::move_to(const float &x, const float &y, const float &
if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to() feed_value for XY:", feed_value); if (g26_debug_flag) SERIAL_ECHOLNPAIR("in move_to() feed_value for XY:", feed_value);
destination[X_AXIS] = x; destination[X_AXIS] = rx;
destination[Y_AXIS] = y; destination[Y_AXIS] = ry;
destination[E_AXIS] += e_delta; destination[E_AXIS] += e_delta;
G26_line_to_destination(feed_value); G26_line_to_destination(feed_value);
@ -734,9 +734,9 @@ bool unified_bed_leveling::parse_G26_parameters() {
return UBL_ERR; return UBL_ERR;
} }
g26_x_pos = parser.linearval('X', current_position[X_AXIS]); g26_x_pos = parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position[X_AXIS];
g26_y_pos = parser.linearval('Y', current_position[Y_AXIS]); g26_y_pos = parser.seenval('Y') ? RAW_X_POSITION(parser.value_linear_units()) : current_position[Y_AXIS];
if (!position_is_reachable_xy(g26_x_pos, g26_y_pos)) { if (!position_is_reachable(g26_x_pos, g26_y_pos)) {
SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds."); SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds.");
return UBL_ERR; return UBL_ERR;
} }

42
Marlin/src/feature/bedlevel/ubl/ubl.h
View file

@ -108,14 +108,14 @@ class unified_bed_leveling {
static bool g29_parameter_parsing(); static bool g29_parameter_parsing();
static void find_mean_mesh_height(); static void find_mean_mesh_height();
static void shift_mesh_height(); static void shift_mesh_height();
static void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest); static void probe_entire_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool); static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3); static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
static void g29_what_command(); static void g29_what_command();
static void g29_eeprom_dump(); static void g29_eeprom_dump();
static void g29_compare_current_mesh_to_stored_mesh(); static void g29_compare_current_mesh_to_stored_mesh();
static void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map); static void fine_tune_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map);
static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir); static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
static void smart_fill_mesh(); static void smart_fill_mesh();
@ -243,12 +243,12 @@ class unified_bed_leveling {
* z_correction_for_x_on_horizontal_mesh_line is an optimization for * z_correction_for_x_on_horizontal_mesh_line is an optimization for
* the case where the printer is making a vertical line that only crosses horizontal mesh lines. * the case where the printer is making a vertical line that only crosses horizontal mesh lines.
*/ */
inline static float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) { inline static float z_correction_for_x_on_horizontal_mesh_line(const float &rx0, const int x1_i, const int yi) {
if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) { if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") ); serialprintPGM( !WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) ? PSTR("x1l_i") : PSTR("yi") );
SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0); SERIAL_ECHOPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0);
SERIAL_ECHOPAIR(",x1_i=", x1_i); SERIAL_ECHOPAIR(",x1_i=", x1_i);
SERIAL_ECHOPAIR(",yi=", yi); SERIAL_ECHOPAIR(",yi=", yi);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
@ -258,7 +258,7 @@ class unified_bed_leveling {
return NAN; return NAN;
} }
const float xratio = (RAW_X_POSITION(lx0) - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)), const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)),
z1 = z_values[x1_i][yi]; z1 = z_values[x1_i][yi];
return z1 + xratio * (z_values[x1_i + 1][yi] - z1); return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
@ -267,12 +267,12 @@ class unified_bed_leveling {
// //
// See comments above for z_correction_for_x_on_horizontal_mesh_line // See comments above for z_correction_for_x_on_horizontal_mesh_line
// //
inline static float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) { inline static float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) {
if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) { if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 2)) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") ); serialprintPGM( !WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) ? PSTR("xi") : PSTR("yl_i") );
SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ly0=", ly0); SERIAL_ECHOPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0);
SERIAL_ECHOPAIR(", xi=", xi); SERIAL_ECHOPAIR(", xi=", xi);
SERIAL_ECHOPAIR(", y1_i=", y1_i); SERIAL_ECHOPAIR(", y1_i=", y1_i);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
@ -282,7 +282,7 @@ class unified_bed_leveling {
return NAN; return NAN;
} }
const float yratio = (RAW_Y_POSITION(ly0) - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)), const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)),
z1 = z_values[xi][y1_i]; z1 = z_values[xi][y1_i];
return z1 + yratio * (z_values[xi][y1_i + 1] - z1); return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
@ -294,14 +294,14 @@ class unified_bed_leveling {
* Z-Height at both ends. Then it does a linear interpolation of these heights based * Z-Height at both ends. Then it does a linear interpolation of these heights based
* on the Y position within the cell. * on the Y position within the cell.
*/ */
static float get_z_correction(const float &lx0, const float &ly0) { static float get_z_correction(const float &rx0, const float &ry0) {
const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)), const int8_t cx = get_cell_index_x(rx0),
cy = get_cell_index_y(RAW_Y_POSITION(ly0)); cy = get_cell_index_y(ry0);
if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) { if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 2) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 2)) {
SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0); SERIAL_ECHOPAIR("? in get_z_correction(rx0=", rx0);
SERIAL_ECHOPAIR(", ly0=", ly0); SERIAL_ECHOPAIR(", ry0=", ry0);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL(); SERIAL_EOL();
@ -312,23 +312,23 @@ class unified_bed_leveling {
return NAN; return NAN;
} }
const float z1 = calc_z0(RAW_X_POSITION(lx0), const float z1 = calc_z0(rx0,
mesh_index_to_xpos(cx), z_values[cx][cy], mesh_index_to_xpos(cx), z_values[cx][cy],
mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]); mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy]);
const float z2 = calc_z0(RAW_X_POSITION(lx0), const float z2 = calc_z0(rx0,
mesh_index_to_xpos(cx), z_values[cx][cy + 1], mesh_index_to_xpos(cx), z_values[cx][cy + 1],
mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]); mesh_index_to_xpos(cx + 1), z_values[cx + 1][cy + 1]);
float z0 = calc_z0(RAW_Y_POSITION(ly0), float z0 = calc_z0(ry0,
mesh_index_to_ypos(cy), z1, mesh_index_to_ypos(cy), z1,
mesh_index_to_ypos(cy + 1), z2); mesh_index_to_ypos(cy + 1), z2);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
SERIAL_ECHOPAIR(" raw get_z_correction(", lx0); SERIAL_ECHOPAIR(" raw get_z_correction(", rx0);
SERIAL_CHAR(','); SERIAL_CHAR(',');
SERIAL_ECHO(ly0); SERIAL_ECHO(ry0);
SERIAL_ECHOPGM(") = "); SERIAL_ECHOPGM(") = ");
SERIAL_ECHO_F(z0, 6); SERIAL_ECHO_F(z0, 6);
} }
@ -350,9 +350,9 @@ class unified_bed_leveling {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(MESH_ADJUST)) { if (DEBUGGING(MESH_ADJUST)) {
SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", lx0); SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", rx0);
SERIAL_CHAR(','); SERIAL_CHAR(',');
SERIAL_ECHO(ly0); SERIAL_ECHO(ry0);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL(); SERIAL_EOL();
} }
@ -369,7 +369,7 @@ class unified_bed_leveling {
return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST); return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
} }
static bool prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate); static bool prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate);
static void line_to_destination_cartesian(const float &fr, uint8_t e); static void line_to_destination_cartesian(const float &fr, uint8_t e);
#define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1]) #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])

91
Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
View file

@ -393,11 +393,11 @@
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
} }
else { // grid_size == 0 : A 3-Point leveling has been requested else { // grid_size == 0 : A 3-Point leveling has been requested
float z3, z2, z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level); float z3, z2, z1 = probe_pt(UBL_PROBE_PT_1_X, UBL_PROBE_PT_1_Y, false, g29_verbose_level);
if (!isnan(z1)) { if (!isnan(z1)) {
z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level); z2 = probe_pt(UBL_PROBE_PT_2_X, UBL_PROBE_PT_2_Y, false, g29_verbose_level);
if (!isnan(z2)) if (!isnan(z2))
z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level); z3 = probe_pt(UBL_PROBE_PT_3_X, UBL_PROBE_PT_3_Y, true, g29_verbose_level);
} }
if (isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable if (isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
@ -411,9 +411,9 @@
// its height is.) // its height is.)
save_ubl_active_state_and_disable(); save_ubl_active_state_and_disable();
z1 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ; z1 -= get_z_correction(UBL_PROBE_PT_1_X, UBL_PROBE_PT_1_Y) /* + zprobe_zoffset */ ;
z2 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ; z2 -= get_z_correction(UBL_PROBE_PT_2_X, UBL_PROBE_PT_2_Y) /* + zprobe_zoffset */ ;
z3 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ; z3 -= get_z_correction(UBL_PROBE_PT_3_X, UBL_PROBE_PT_3_Y) /* + zprobe_zoffset */ ;
do_blocking_move_to_xy(0.5 * (MESH_MAX_X - (MESH_MIN_X)), 0.5 * (MESH_MAX_Y - (MESH_MIN_Y))); do_blocking_move_to_xy(0.5 * (MESH_MAX_X - (MESH_MIN_X)), 0.5 * (MESH_MAX_Y - (MESH_MIN_Y)));
tilt_mesh_based_on_3pts(z1, z2, z3); tilt_mesh_based_on_3pts(z1, z2, z3);
@ -497,7 +497,7 @@
} }
} }
if (!position_is_reachable_xy(g29_x_pos, g29_y_pos)) { if (!position_is_reachable(g29_x_pos, g29_y_pos)) {
SERIAL_PROTOCOLLNPGM("XY outside printable radius."); SERIAL_PROTOCOLLNPGM("XY outside printable radius.");
return; return;
} }
@ -734,7 +734,7 @@
* Probe all invalidated locations of the mesh that can be reached by the probe. * Probe all invalidated locations of the mesh that can be reached by the probe.
* This attempts to fill in locations closest to the nozzle's start location first. * This attempts to fill in locations closest to the nozzle's start location first.
*/ */
void unified_bed_leveling::probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) { void unified_bed_leveling::probe_entire_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) {
mesh_index_pair location; mesh_index_pair location;
has_control_of_lcd_panel = true; has_control_of_lcd_panel = true;
@ -762,13 +762,13 @@
if (close_or_far) if (close_or_far)
location = find_furthest_invalid_mesh_point(); location = find_furthest_invalid_mesh_point();
else else
location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL); location = find_closest_mesh_point_of_type(INVALID, rx, ry, USE_PROBE_AS_REFERENCE, NULL);
if (location.x_index >= 0) { // mesh point found and is reachable by probe if (location.x_index >= 0) { // mesh point found and is reachable by probe
const float rawx = mesh_index_to_xpos(location.x_index), const float rawx = mesh_index_to_xpos(location.x_index),
rawy = mesh_index_to_ypos(location.y_index); rawy = mesh_index_to_ypos(location.y_index);
const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level); // TODO: Needs error handling const float measured_z = probe_pt(rawx, rawy, stow_probe, g29_verbose_level); // TODO: Needs error handling
z_values[location.x_index][location.y_index] = measured_z; z_values[location.x_index][location.y_index] = measured_z;
} }
@ -778,8 +778,8 @@
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
do_blocking_move_to_xy( do_blocking_move_to_xy(
constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), MESH_MIN_X, MESH_MAX_X), constrain(rx - (X_PROBE_OFFSET_FROM_EXTRUDER), MESH_MIN_X, MESH_MAX_X),
constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), MESH_MIN_Y, MESH_MAX_Y) constrain(ry - (Y_PROBE_OFFSET_FROM_EXTRUDER), MESH_MIN_Y, MESH_MAX_Y)
); );
} }
@ -953,28 +953,26 @@
return thickness; return thickness;
} }
void unified_bed_leveling::manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) { void unified_bed_leveling::manually_probe_remaining_mesh(const float &rx, const float &ry, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) {
has_control_of_lcd_panel = true; has_control_of_lcd_panel = true;
save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to_xy(lx, ly); do_blocking_move_to_xy(rx, ry);
lcd_return_to_status(); lcd_return_to_status();
mesh_index_pair location; mesh_index_pair location;
do { do {
location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_NOZZLE_AS_REFERENCE, NULL); location = find_closest_mesh_point_of_type(INVALID, rx, ry, USE_NOZZLE_AS_REFERENCE, NULL);
// It doesn't matter if the probe can't reach the NAN location. This is a manual probe. // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
if (location.x_index < 0 && location.y_index < 0) continue; if (location.x_index < 0 && location.y_index < 0) continue;
const float rawx = mesh_index_to_xpos(location.x_index), const float xProbe = mesh_index_to_xpos(location.x_index),
rawy = mesh_index_to_ypos(location.y_index), yProbe = mesh_index_to_ypos(location.y_index);
xProbe = LOGICAL_X_POSITION(rawx),
yProbe = LOGICAL_Y_POSITION(rawy);
if (!position_is_reachable_raw_xy(rawx, rawy)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points) if (!position_is_reachable(xProbe, yProbe)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
@ -1038,7 +1036,7 @@
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
KEEPALIVE_STATE(IN_HANDLER); KEEPALIVE_STATE(IN_HANDLER);
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
do_blocking_move_to_xy(lx, ly); do_blocking_move_to_xy(rx, ry);
} }
#endif // NEWPANEL #endif // NEWPANEL
@ -1111,8 +1109,8 @@
} }
// If X or Y are not valid, use center of the bed values // If X or Y are not valid, use center of the bed values
if (!WITHIN(RAW_X_POSITION(g29_x_pos), X_MIN_BED, X_MAX_BED)) g29_x_pos = LOGICAL_X_POSITION(X_CENTER); if (!WITHIN(g29_x_pos, X_MIN_BED, X_MAX_BED)) g29_x_pos = X_CENTER;
if (!WITHIN(RAW_Y_POSITION(g29_y_pos), Y_MIN_BED, Y_MAX_BED)) g29_y_pos = LOGICAL_Y_POSITION(Y_CENTER); if (!WITHIN(g29_y_pos, Y_MIN_BED, Y_MAX_BED)) g29_y_pos = Y_CENTER;
if (err_flag) return UBL_ERR; if (err_flag) return UBL_ERR;
@ -1365,7 +1363,7 @@
const float mx = mesh_index_to_xpos(i), const float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j); my = mesh_index_to_ypos(j);
if ( !position_is_reachable_by_probe_raw_xy(mx, my)) // make sure the probe can get to the mesh point if ( !position_is_reachable_by_probe(mx, my)) // make sure the probe can get to the mesh point
continue; continue;
found_a_NAN = true; found_a_NAN = true;
@ -1413,14 +1411,14 @@
return out_mesh; return out_mesh;
} }
mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, uint16_t bits[16]) { mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &rx, const float &ry, const bool probe_as_reference, uint16_t bits[16]) {
mesh_index_pair out_mesh; mesh_index_pair out_mesh;
out_mesh.x_index = out_mesh.y_index = -1; out_mesh.x_index = out_mesh.y_index = -1;
out_mesh.distance = -99999.9; out_mesh.distance = -99999.9;
// Get our reference position. Either the nozzle or probe location. // Get our reference position. Either the nozzle or probe location.
const float px = RAW_X_POSITION(lx) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0), const float px = rx - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
py = RAW_Y_POSITION(ly) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0); py = ry - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0);
float best_so_far = 99999.99; float best_so_far = 99999.99;
@ -1433,7 +1431,6 @@
) { ) {
// We only get here if we found a Mesh Point of the specified type // We only get here if we found a Mesh Point of the specified type
float raw_x = RAW_CURRENT_POSITION(X), raw_y = RAW_CURRENT_POSITION(Y);
const float mx = mesh_index_to_xpos(i), const float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j); my = mesh_index_to_ypos(j);
@ -1441,7 +1438,7 @@
// Also for round beds, there are grid points outside the bed the nozzle can't reach. // Also for round beds, there are grid points outside the bed the nozzle can't reach.
// Prune them from the list and ignore them till the next Phase (manual nozzle probing). // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
if (probe_as_reference ? !position_is_reachable_by_probe_raw_xy(mx, my) : !position_is_reachable_raw_xy(mx, my)) if (probe_as_reference ? !position_is_reachable_by_probe(mx, my) : !position_is_reachable(mx, my))
continue; continue;
// Reachable. Check if it's the best_so_far location to the nozzle. // Reachable. Check if it's the best_so_far location to the nozzle.
@ -1450,7 +1447,7 @@
// factor in the distance from the current location for the normal case // factor in the distance from the current location for the normal case
// so the nozzle isn't running all over the bed. // so the nozzle isn't running all over the bed.
distance += HYPOT(raw_x - mx, raw_y - my) * 0.1; distance += HYPOT(current_position[X_AXIS] - mx, current_position[Y_AXIS] - my) * 0.1;
if (distance < best_so_far) { if (distance < best_so_far) {
best_so_far = distance; // We found a closer location with best_so_far = distance; // We found a closer location with
out_mesh.x_index = i; // the specified type of mesh value. out_mesh.x_index = i; // the specified type of mesh value.
@ -1465,7 +1462,7 @@
#if ENABLED(NEWPANEL) #if ENABLED(NEWPANEL)
void unified_bed_leveling::fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) { void unified_bed_leveling::fine_tune_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map) {
if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified
g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided. g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided.
@ -1480,7 +1477,7 @@
mesh_index_pair location; mesh_index_pair location;
if (!position_is_reachable_xy(lx, ly)) { if (!position_is_reachable(rx, ry)) {
SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius."); SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
return; return;
} }
@ -1490,12 +1487,12 @@
LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH); LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH);
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to_xy(lx, ly); do_blocking_move_to_xy(rx, ry);
uint16_t not_done[16]; uint16_t not_done[16];
memset(not_done, 0xFF, sizeof(not_done)); memset(not_done, 0xFF, sizeof(not_done));
do { do {
location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done); location = find_closest_mesh_point_of_type(SET_IN_BITMAP, rx, ry, USE_NOZZLE_AS_REFERENCE, not_done);
if (location.x_index < 0) break; // stop when we can't find any more reachable points. if (location.x_index < 0) break; // stop when we can't find any more reachable points.
@ -1505,7 +1502,7 @@
const float rawx = mesh_index_to_xpos(location.x_index), const float rawx = mesh_index_to_xpos(location.x_index),
rawy = mesh_index_to_ypos(location.y_index); rawy = mesh_index_to_ypos(location.y_index);
if (!position_is_reachable_raw_xy(rawx, rawy)) // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable if (!position_is_reachable(rawx, rawy)) // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable
break; break;
float new_z = z_values[location.x_index][location.y_index]; float new_z = z_values[location.x_index][location.y_index];
@ -1514,7 +1511,7 @@
new_z = 0.0; new_z = 0.0;
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); // Move the nozzle to where we are going to edit do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); // Move the nozzle to where we are going to edit
do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy)); do_blocking_move_to_xy(rawx, rawy);
new_z = FLOOR(new_z * 1000.0) * 0.001; // Chop off digits after the 1000ths place new_z = FLOOR(new_z * 1000.0) * 0.001; // Chop off digits after the 1000ths place
@ -1576,7 +1573,7 @@
restore_ubl_active_state_and_leave(); restore_ubl_active_state_and_leave();
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to_xy(lx, ly); do_blocking_move_to_xy(rx, ry);
LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH); LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH);
SERIAL_ECHOLNPGM("Done Editing Mesh"); SERIAL_ECHOLNPGM("Done Editing Mesh");
@ -1654,29 +1651,29 @@
bool zig_zag = false; bool zig_zag = false;
for (uint8_t ix = 0; ix < g29_grid_size; ix++) { for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
const float x = float(x_min) + ix * dx; const float rx = float(x_min) + ix * dx;
for (int8_t iy = 0; iy < g29_grid_size; iy++) { for (int8_t iy = 0; iy < g29_grid_size; iy++) {
const float y = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy); const float ry = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), parser.seen('E'), g29_verbose_level); // TODO: Needs error handling float measured_z = probe_pt(rx, ry, parser.seen('E'), g29_verbose_level); // TODO: Needs error handling
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_CHAR('('); SERIAL_CHAR('(');
SERIAL_PROTOCOL_F(x, 7); SERIAL_PROTOCOL_F(rx, 7);
SERIAL_CHAR(','); SERIAL_CHAR(',');
SERIAL_PROTOCOL_F(y, 7); SERIAL_PROTOCOL_F(ry, 7);
SERIAL_ECHOPGM(") logical: "); SERIAL_ECHOPGM(") logical: ");
SERIAL_CHAR('('); SERIAL_CHAR('(');
SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(rx), 7);
SERIAL_CHAR(','); SERIAL_CHAR(',');
SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(ry), 7);
SERIAL_ECHOPGM(") measured: "); SERIAL_ECHOPGM(") measured: ");
SERIAL_PROTOCOL_F(measured_z, 7); SERIAL_PROTOCOL_F(measured_z, 7);
SERIAL_ECHOPGM(" correction: "); SERIAL_ECHOPGM(" correction: ");
SERIAL_PROTOCOL_F(get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7); SERIAL_PROTOCOL_F(get_z_correction(rx, ry), 7);
} }
#endif #endif
measured_z -= get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ; measured_z -= get_z_correction(rx, ry) /* + zprobe_zoffset */ ;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
@ -1686,7 +1683,7 @@
} }
#endif #endif
incremental_LSF(&lsf_results, x, y, measured_z); incremental_LSF(&lsf_results, rx, ry, measured_z);
} }
zig_zag ^= true; zig_zag ^= true;

127
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View file

@ -112,10 +112,10 @@
destination[E_AXIS] destination[E_AXIS]
}; };
const int cell_start_xi = get_cell_index_x(RAW_X_POSITION(start[X_AXIS])), const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(RAW_Y_POSITION(start[Y_AXIS])), cell_start_yi = get_cell_index_y(start[Y_AXIS]),
cell_dest_xi = get_cell_index_x(RAW_X_POSITION(end[X_AXIS])), cell_dest_xi = get_cell_index_x(end[X_AXIS]),
cell_dest_yi = get_cell_index_y(RAW_Y_POSITION(end[Y_AXIS])); cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
if (g26_debug_flag) { if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]); SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
@ -160,7 +160,7 @@
* to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
*/ */
const float xratio = (RAW_X_POSITION(end[X_AXIS]) - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)); const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio * float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]), (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
@ -172,7 +172,7 @@
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
// are going to apply the Y-Distance into the cell to interpolate the final Z correction. // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
const float yratio = (RAW_Y_POSITION(end[Y_AXIS]) - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST)); const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0; float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
/** /**
@ -248,16 +248,16 @@
current_yi += down_flag; // Line is heading down, we just want to go to the bottom current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) { while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi; current_yi += dyi;
const float next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi)); const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
/** /**
* if the slope of the line is infinite, we won't do the calculations * if the slope of the line is infinite, we won't do the calculations
* else, we know the next X is the same so we can recover and continue! * else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line * Calculate X at the next Y mesh line
*/ */
const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi) float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]); * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
@ -269,7 +269,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
const float y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi)); const float ry = mesh_index_to_ypos(current_yi);
/** /**
* Without this check, it is possible for the algorithm to generate a zero length move in the case * Without this check, it is possible for the algorithm to generate a zero length move in the case
@ -277,9 +277,9 @@
* happens, it might be best to remove the check and always 'schedule' the move because * happens, it might be best to remove the check and always 'schedule' the move because
* the planner._buffer_line() routine will filter it if that happens. * the planner._buffer_line() routine will filter it if that happens.
*/ */
if (y != start[Y_AXIS]) { if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
@ -288,7 +288,7 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder); planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
@ -318,10 +318,10 @@
// edge of this cell for the first move. // edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) { while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi; current_xi += dxi;
const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi)), const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi) float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]); * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
@ -333,7 +333,7 @@
*/ */
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
const float x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi)); const float rx = mesh_index_to_xpos(current_xi);
/** /**
* Without this check, it is possible for the algorithm to generate a zero length move in the case * Without this check, it is possible for the algorithm to generate a zero length move in the case
@ -341,9 +341,9 @@
* that happens, it might be best to remove the check and always 'schedule' the move because * that happens, it might be best to remove the check and always 'schedule' the move because
* the planner._buffer_line() routine will filter it if that happens. * the planner._buffer_line() routine will filter it if that happens.
*/ */
if (x != start[X_AXIS]) { if (rx != start[X_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
@ -352,7 +352,7 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder); planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
@ -383,17 +383,17 @@
while (xi_cnt > 0 || yi_cnt > 0) { while (xi_cnt > 0 || yi_cnt > 0) {
const float next_mesh_line_x = LOGICAL_X_POSITION(mesh_index_to_xpos(current_xi + dxi)), const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
next_mesh_line_y = LOGICAL_Y_POSITION(mesh_index_to_ypos(current_yi + dyi)), next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero. // (No need to worry about m being zero.
// If that was the case, it was already detected // If that was the case, it was already detected
// as a vertical line move above.) // as a vertical line move above.)
if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
// Yes! Crossing a Y Mesh Line next // Yes! Crossing a Y Mesh Line next
float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi) float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]); * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
@ -406,7 +406,7 @@
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS]; on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
@ -414,13 +414,13 @@
e_position = end[E_AXIS]; e_position = end[E_AXIS];
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(x, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
current_yi += dyi; current_yi += dyi;
yi_cnt--; yi_cnt--;
} }
else { else {
// Yes! Crossing a X Mesh Line next // Yes! Crossing a X Mesh Line next
float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag) float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]); * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/** /**
@ -433,7 +433,7 @@
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS]; on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
} }
@ -442,7 +442,7 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(next_mesh_line_x, y, z_position + z0, e_position, feed_rate, extruder); planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
current_xi += dxi; current_xi += dxi;
xi_cnt--; xi_cnt--;
} }
@ -476,7 +476,7 @@
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, // 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 and kinematics performed first. // so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw( float rx, float ry, float rz, float le, float fr ) { inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) {
#if ENABLED(DELTA) // apply delta inverse_kinematics #if ENABLED(DELTA) // apply delta inverse_kinematics
@ -492,14 +492,11 @@
- HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx, - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
delta_tower[C_AXIS][Y_AXIS] - ry )); delta_tower[C_AXIS][Y_AXIS] - ry ));
planner._buffer_line(delta_A, delta_B, delta_C, le, fr, active_extruder); planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
const float lseg[XYZ] = { LOGICAL_X_POSITION(rx), const float lseg[XYZ] = { rx, ry, rz };
LOGICAL_Y_POSITION(ry),
LOGICAL_Z_POSITION(rz)
};
inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ] inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
// should move the feedrate scaling to scara inverse_kinematics // should move the feedrate scaling to scara inverse_kinematics
@ -510,17 +507,11 @@
scara_oldB = delta[B_AXIS]; scara_oldB = delta[B_AXIS];
float s_feedrate = max(adiff, bdiff) * scara_feed_factor; 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); planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], e, s_feedrate, active_extruder);
#else // CARTESIAN #else // CARTESIAN
// Cartesian _buffer_line seems to take LOGICAL, not RAW coordinates planner._buffer_line(rx, ry, rz, e, fr, active_extruder);
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 #endif
@ -533,15 +524,15 @@
* Returns true if did NOT move, false if moved (requires current_position update). * Returns true if did NOT move, false if moved (requires current_position update).
*/ */
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float ltarget[XYZE], const float &feedrate) { bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) {
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) // fail if moving outside reachable boundary if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate return true; // did not move, so current_position still accurate
const float tot_dx = ltarget[X_AXIS] - current_position[X_AXIS], const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS],
tot_dy = ltarget[Y_AXIS] - current_position[Y_AXIS], tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS],
tot_dz = ltarget[Z_AXIS] - current_position[Z_AXIS], tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS],
tot_de = ltarget[E_AXIS] - current_position[E_AXIS]; tot_de = rtarget[E_AXIS] - current_position[E_AXIS];
const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance
@ -571,14 +562,14 @@
// Note that E segment distance could vary slightly as z mesh height // Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore. // changes for each segment, but small enough to ignore.
float seg_rx = RAW_X_POSITION(current_position[X_AXIS]), float seg_rx = current_position[X_AXIS],
seg_ry = RAW_Y_POSITION(current_position[Y_AXIS]), seg_ry = current_position[Y_AXIS],
seg_rz = RAW_Z_POSITION(current_position[Z_AXIS]), seg_rz = current_position[Z_AXIS],
seg_le = current_position[E_AXIS]; seg_le = current_position[E_AXIS];
// Only compute leveling per segment if ubl active and target below z_fade_height. // Only compute leveling per segment if ubl active and target below z_fade_height.
if (!planner.leveling_active || !planner.leveling_active_at_z(ltarget[Z_AXIS])) { // no mesh leveling if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
do { do {
@ -588,13 +579,13 @@
seg_rz += seg_dz; seg_rz += seg_dz;
seg_le += seg_de; seg_le += seg_de;
} else { // last segment, use exact destination } else { // last segment, use exact destination
seg_rx = RAW_X_POSITION(ltarget[X_AXIS]); seg_rx = rtarget[X_AXIS];
seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]); seg_ry = rtarget[Y_AXIS];
seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]); seg_rz = rtarget[Z_AXIS];
seg_le = ltarget[E_AXIS]; seg_le = rtarget[E_AXIS];
} }
ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz, seg_le, feedrate ); ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate);
} while (segments); } while (segments);
@ -604,7 +595,7 @@
// Otherwise perform per-segment leveling // Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(ltarget[Z_AXIS]); const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
#endif #endif
// increment to first segment destination // increment to first segment destination
@ -671,16 +662,16 @@
z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height z_cxcy *= fade_scaling_factor; // apply fade factor to interpolated mesh height
#endif #endif
if (--segments == 0) { // if this is last segment, use ltarget for exact if (--segments == 0) { // if this is last segment, use rtarget for exact
seg_rx = RAW_X_POSITION(ltarget[X_AXIS]); seg_rx = rtarget[X_AXIS];
seg_ry = RAW_Y_POSITION(ltarget[Y_AXIS]); seg_ry = rtarget[Y_AXIS];
seg_rz = RAW_Z_POSITION(ltarget[Z_AXIS]); seg_rz = rtarget[Z_AXIS];
seg_le = ltarget[E_AXIS]; seg_le = rtarget[E_AXIS];
} }
ubl_buffer_segment_raw( seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate ); ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
if (segments == 0 ) // done with last segment if (segments == 0) // done with last segment
return false; // did not set_current_from_destination() return false; // did not set_current_from_destination()
seg_rx += seg_dx; seg_rx += seg_dx;

4
Marlin/src/gcode/bedlevel/G42.cpp
View file

@ -56,8 +56,8 @@ void GcodeSuite::G42() {
#endif #endif
set_destination_from_current(); set_destination_from_current();
if (hasI) destination[X_AXIS] = LOGICAL_X_POSITION(_GET_MESH_X(ix)); if (hasI) destination[X_AXIS] = _GET_MESH_X(ix);
if (hasJ) destination[Y_AXIS] = LOGICAL_Y_POSITION(_GET_MESH_Y(iy)); if (hasJ) destination[Y_AXIS] = _GET_MESH_Y(iy);
if (parser.boolval('P')) { if (parser.boolval('P')) {
if (hasI) destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER; if (hasI) destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
if (hasJ) destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER; if (hasJ) destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;

62
Marlin/src/gcode/bedlevel/abl/G29.cpp
View file

@ -258,28 +258,28 @@ void GcodeSuite::G29() {
return; return;
} }
const float z = parser.floatval('Z', RAW_CURRENT_POSITION(Z)); const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position[Z_AXIS];
if (!WITHIN(z, -10, 10)) { if (!WITHIN(rz, -10, 10)) {
SERIAL_ERROR_START(); SERIAL_ERROR_START();
SERIAL_ERRORLNPGM("Bad Z value"); SERIAL_ERRORLNPGM("Bad Z value");
return; return;
} }
const float x = parser.floatval('X', NAN), const float rx = RAW_X_POSITION(parser.linearval('X', NAN)),
y = parser.floatval('Y', NAN); ry = RAW_Y_POSITION(parser.linearval('Y', NAN));
int8_t i = parser.byteval('I', -1), int8_t i = parser.byteval('I', -1),
j = parser.byteval('J', -1); j = parser.byteval('J', -1);
if (!isnan(x) && !isnan(y)) { if (!isnan(rx) && !isnan(ry)) {
// Get nearest i / j from x / y // Get nearest i / j from rx / ry
i = (x - LOGICAL_X_POSITION(bilinear_start[X_AXIS]) + 0.5 * xGridSpacing) / xGridSpacing; i = (rx - bilinear_start[X_AXIS] + 0.5 * xGridSpacing) / xGridSpacing;
j = (y - LOGICAL_Y_POSITION(bilinear_start[Y_AXIS]) + 0.5 * yGridSpacing) / yGridSpacing; j = (ry - bilinear_start[Y_AXIS] + 0.5 * yGridSpacing) / yGridSpacing;
i = constrain(i, 0, GRID_MAX_POINTS_X - 1); i = constrain(i, 0, GRID_MAX_POINTS_X - 1);
j = constrain(j, 0, GRID_MAX_POINTS_Y - 1); j = constrain(j, 0, GRID_MAX_POINTS_Y - 1);
} }
if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) { if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) {
set_bed_leveling_enabled(false); set_bed_leveling_enabled(false);
z_values[i][j] = z; z_values[i][j] = rz;
#if ENABLED(ABL_BILINEAR_SUBDIVISION) #if ENABLED(ABL_BILINEAR_SUBDIVISION)
bed_level_virt_interpolate(); bed_level_virt_interpolate();
#endif #endif
@ -340,36 +340,36 @@ void GcodeSuite::G29() {
xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED)); xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_SPEED));
left_probe_bed_position = (int)parser.linearval('L', LOGICAL_X_POSITION(LEFT_PROBE_BED_POSITION)); left_probe_bed_position = parser.seenval('L') ? (int)RAW_X_POSITION(parser.value_linear_units()) : LEFT_PROBE_BED_POSITION;
right_probe_bed_position = (int)parser.linearval('R', LOGICAL_X_POSITION(RIGHT_PROBE_BED_POSITION)); right_probe_bed_position = parser.seenval('R') ? (int)RAW_X_POSITION(parser.value_linear_units()) : RIGHT_PROBE_BED_POSITION;
front_probe_bed_position = (int)parser.linearval('F', LOGICAL_Y_POSITION(FRONT_PROBE_BED_POSITION)); front_probe_bed_position = parser.seenval('F') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : FRONT_PROBE_BED_POSITION;
back_probe_bed_position = (int)parser.linearval('B', LOGICAL_Y_POSITION(BACK_PROBE_BED_POSITION)); back_probe_bed_position = parser.seenval('B') ? (int)RAW_Y_POSITION(parser.value_linear_units()) : BACK_PROBE_BED_POSITION;
const bool left_out_l = left_probe_bed_position < LOGICAL_X_POSITION(MIN_PROBE_X), const bool left_out_l = left_probe_bed_position < MIN_PROBE_X,
left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - (MIN_PROBE_EDGE), left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - (MIN_PROBE_EDGE),
right_out_r = right_probe_bed_position > LOGICAL_X_POSITION(MAX_PROBE_X), right_out_r = right_probe_bed_position > MAX_PROBE_X,
right_out = right_out_r || right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE, right_out = right_out_r || right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE,
front_out_f = front_probe_bed_position < LOGICAL_Y_POSITION(MIN_PROBE_Y), front_out_f = front_probe_bed_position < MIN_PROBE_Y,
front_out = front_out_f || front_probe_bed_position > back_probe_bed_position - (MIN_PROBE_EDGE), front_out = front_out_f || front_probe_bed_position > back_probe_bed_position - (MIN_PROBE_EDGE),
back_out_b = back_probe_bed_position > LOGICAL_Y_POSITION(MAX_PROBE_Y), back_out_b = back_probe_bed_position > MAX_PROBE_Y,
back_out = back_out_b || back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE; back_out = back_out_b || back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE;
if (left_out || right_out || front_out || back_out) { if (left_out || right_out || front_out || back_out) {
if (left_out) { if (left_out) {
out_of_range_error(PSTR("(L)eft")); out_of_range_error(PSTR("(L)eft"));
left_probe_bed_position = left_out_l ? LOGICAL_X_POSITION(MIN_PROBE_X) : right_probe_bed_position - (MIN_PROBE_EDGE); left_probe_bed_position = left_out_l ? MIN_PROBE_X : right_probe_bed_position - (MIN_PROBE_EDGE);
} }
if (right_out) { if (right_out) {
out_of_range_error(PSTR("(R)ight")); out_of_range_error(PSTR("(R)ight"));
right_probe_bed_position = right_out_r ? LOGICAL_Y_POSITION(MAX_PROBE_X) : left_probe_bed_position + MIN_PROBE_EDGE; right_probe_bed_position = right_out_r ? MAX_PROBE_X : left_probe_bed_position + MIN_PROBE_EDGE;
} }
if (front_out) { if (front_out) {
out_of_range_error(PSTR("(F)ront")); out_of_range_error(PSTR("(F)ront"));
front_probe_bed_position = front_out_f ? LOGICAL_Y_POSITION(MIN_PROBE_Y) : back_probe_bed_position - (MIN_PROBE_EDGE); front_probe_bed_position = front_out_f ? MIN_PROBE_Y : back_probe_bed_position - (MIN_PROBE_EDGE);
} }
if (back_out) { if (back_out) {
out_of_range_error(PSTR("(B)ack")); out_of_range_error(PSTR("(B)ack"));
back_probe_bed_position = back_out_b ? LOGICAL_Y_POSITION(MAX_PROBE_Y) : front_probe_bed_position + MIN_PROBE_EDGE; back_probe_bed_position = back_out_b ? MAX_PROBE_Y : front_probe_bed_position + MIN_PROBE_EDGE;
} }
return; return;
} }
@ -416,8 +416,8 @@ void GcodeSuite::G29() {
#endif #endif
if ( xGridSpacing != bilinear_grid_spacing[X_AXIS] if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
|| yGridSpacing != bilinear_grid_spacing[Y_AXIS] || yGridSpacing != bilinear_grid_spacing[Y_AXIS]
|| left_probe_bed_position != LOGICAL_X_POSITION(bilinear_start[X_AXIS]) || left_probe_bed_position != bilinear_start[X_AXIS]
|| front_probe_bed_position != LOGICAL_Y_POSITION(bilinear_start[Y_AXIS]) || front_probe_bed_position != bilinear_start[Y_AXIS]
) { ) {
if (dryrun) { if (dryrun) {
// Before reset bed level, re-enable to correct the position // Before reset bed level, re-enable to correct the position
@ -429,8 +429,8 @@ void GcodeSuite::G29() {
// Initialize a grid with the given dimensions // Initialize a grid with the given dimensions
bilinear_grid_spacing[X_AXIS] = xGridSpacing; bilinear_grid_spacing[X_AXIS] = xGridSpacing;
bilinear_grid_spacing[Y_AXIS] = yGridSpacing; bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
bilinear_start[X_AXIS] = RAW_X_POSITION(left_probe_bed_position); bilinear_start[X_AXIS] = left_probe_bed_position;
bilinear_start[Y_AXIS] = RAW_Y_POSITION(front_probe_bed_position); bilinear_start[Y_AXIS] = front_probe_bed_position;
// Can't re-enable (on error) until the new grid is written // Can't re-enable (on error) until the new grid is written
abl_should_enable = false; abl_should_enable = false;
@ -555,7 +555,7 @@ void GcodeSuite::G29() {
#endif #endif
// Keep looping till a reachable point is found // Keep looping till a reachable point is found
if (position_is_reachable_xy(xProbe, yProbe)) break; if (position_is_reachable(xProbe, yProbe)) break;
++abl_probe_index; ++abl_probe_index;
} }
@ -585,8 +585,8 @@ void GcodeSuite::G29() {
// Probe at 3 arbitrary points // Probe at 3 arbitrary points
if (abl_probe_index < 3) { if (abl_probe_index < 3) {
xProbe = LOGICAL_X_POSITION(points[abl_probe_index].x); xProbe = points[abl_probe_index].x;
yProbe = LOGICAL_Y_POSITION(points[abl_probe_index].y); yProbe = points[abl_probe_index].y;
#if HAS_SOFTWARE_ENDSTOPS #if HAS_SOFTWARE_ENDSTOPS
// Disable software endstops to allow manual adjustment // Disable software endstops to allow manual adjustment
// If G29 is not completed, they will not be re-enabled // If G29 is not completed, they will not be re-enabled
@ -663,7 +663,7 @@ void GcodeSuite::G29() {
#if IS_KINEMATIC #if IS_KINEMATIC
// Avoid probing outside the round or hexagonal area // Avoid probing outside the round or hexagonal area
if (!position_is_reachable_by_probe_xy(xProbe, yProbe)) continue; if (!position_is_reachable_by_probe(xProbe, yProbe)) continue;
#endif #endif
measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level); measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
@ -701,8 +701,8 @@ void GcodeSuite::G29() {
for (uint8_t i = 0; i < 3; ++i) { for (uint8_t i = 0; i < 3; ++i) {
// Retain the last probe position // Retain the last probe position
xProbe = LOGICAL_X_POSITION(points[i].x); xProbe = points[i].x;
yProbe = LOGICAL_Y_POSITION(points[i].y); yProbe = points[i].y;
measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level); measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
if (isnan(measured_z)) { if (isnan(measured_z)) {
planner.leveling_active = abl_should_enable; planner.leveling_active = abl_should_enable;

4
Marlin/src/gcode/bedlevel/mbl/G29.cpp
View file

@ -46,7 +46,7 @@ void mesh_probing_done() {
gcode.home_all_axes(); gcode.home_all_axes();
set_bed_leveling_enabled(true); set_bed_leveling_enabled(true);
#if ENABLED(MESH_G28_REST_ORIGIN) #if ENABLED(MESH_G28_REST_ORIGIN)
current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS); current_position[Z_AXIS] = Z_MIN_POS;
set_destination_from_current(); set_destination_from_current();
line_to_destination(homing_feedrate(Z_AXIS)); line_to_destination(homing_feedrate(Z_AXIS));
stepper.synchronize(); stepper.synchronize();
@ -139,7 +139,7 @@ void GcodeSuite::G29() {
} }
else { else {
// One last "return to the bed" (as originally coded) at completion // One last "return to the bed" (as originally coded) at completion
current_position[Z_AXIS] = LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT; current_position[Z_AXIS] = Z_MIN_POS + MANUAL_PROBE_HEIGHT;
line_to_current_position(); line_to_current_position();
stepper.synchronize(); stepper.synchronize();

10
Marlin/src/gcode/calibrate/G28.cpp
View file

@ -86,8 +86,8 @@
/** /**
* Move the Z probe (or just the nozzle) to the safe homing point * Move the Z probe (or just the nozzle) to the safe homing point
*/ */
destination[X_AXIS] = LOGICAL_X_POSITION(Z_SAFE_HOMING_X_POINT); destination[X_AXIS] = Z_SAFE_HOMING_X_POINT;
destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT); destination[Y_AXIS] = Z_SAFE_HOMING_Y_POINT;
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
#if HOMING_Z_WITH_PROBE #if HOMING_Z_WITH_PROBE
@ -95,7 +95,7 @@
destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER; destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
#endif #endif
if (position_is_reachable_xy(destination[X_AXIS], destination[Y_AXIS])) { if (position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination); if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
@ -209,7 +209,7 @@ void GcodeSuite::G28(const bool always_home_all) {
if (home_all || homeX || homeY) { if (home_all || homeX || homeY) {
// Raise Z before homing any other axes and z is not already high enough (never lower z) // Raise Z before homing any other axes and z is not already high enough (never lower z)
destination[Z_AXIS] = LOGICAL_Z_POSITION(Z_HOMING_HEIGHT); destination[Z_AXIS] = Z_HOMING_HEIGHT;
if (destination[Z_AXIS] > current_position[Z_AXIS]) { if (destination[Z_AXIS] > current_position[Z_AXIS]) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
@ -251,7 +251,7 @@ void GcodeSuite::G28(const bool always_home_all) {
HOMEAXIS(X); HOMEAXIS(X);
// Remember this extruder's position for later tool change // Remember this extruder's position for later tool change
inactive_extruder_x_pos = RAW_X_POSITION(current_position[X_AXIS]); inactive_extruder_x_pos = current_position[X_AXIS];
// Home the 1st (left) extruder // Home the 1st (left) extruder
active_extruder = 0; active_extruder = 0;

2
Marlin/src/gcode/calibrate/G33.cpp
View file

@ -459,7 +459,7 @@ void GcodeSuite::G33() {
LOOP_CAL_RAD(axis) { LOOP_CAL_RAD(axis) {
const float a = RADIANS(210 + (360 / NPP) * (axis - 1)), const float a = RADIANS(210 + (360 / NPP) * (axis - 1)),
r = delta_calibration_radius * (1 + (_7p_9_centre ? 0.1 : 0.0)); r = delta_calibration_radius * (1 + (_7p_9_centre ? 0.1 : 0.0));
if (!position_is_reachable_xy(cos(a) * r, sin(a) * r)) { if (!position_is_reachable(cos(a) * r, sin(a) * r)) {
SERIAL_PROTOCOLLNPGM("?(M665 B)ed radius is implausible."); SERIAL_PROTOCOLLNPGM("?(M665 B)ed radius is implausible.");
return; return;
} }

8
Marlin/src/gcode/calibrate/M48.cpp
View file

@ -82,16 +82,16 @@ void GcodeSuite::M48() {
Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER); Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER);
#if DISABLED(DELTA) #if DISABLED(DELTA)
if (!WITHIN(X_probe_location, LOGICAL_X_POSITION(MIN_PROBE_X), LOGICAL_X_POSITION(MAX_PROBE_X))) { if (!WITHIN(X_probe_location, MIN_PROBE_X, MAX_PROBE_X)) {
out_of_range_error(PSTR("X")); out_of_range_error(PSTR("X"));
return; return;
} }
if (!WITHIN(Y_probe_location, LOGICAL_Y_POSITION(MIN_PROBE_Y), LOGICAL_Y_POSITION(MAX_PROBE_Y))) { if (!WITHIN(Y_probe_location, MIN_PROBE_Y, MAX_PROBE_Y)) {
out_of_range_error(PSTR("Y")); out_of_range_error(PSTR("Y"));
return; return;
} }
#else #else
if (!position_is_reachable_by_probe_xy(X_probe_location, Y_probe_location)) { if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius."); SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius.");
return; return;
} }
@ -184,7 +184,7 @@ void GcodeSuite::M48() {
#else #else
// If we have gone out too far, we can do a simple fix and scale the numbers // If we have gone out too far, we can do a simple fix and scale the numbers
// back in closer to the origin. // back in closer to the origin.
while (!position_is_reachable_by_probe_xy(X_current, Y_current)) { while (!position_is_reachable_by_probe(X_current, Y_current)) {
X_current *= 0.8; X_current *= 0.8;
Y_current *= 0.8; Y_current *= 0.8;
if (verbose_level > 3) { if (verbose_level > 3) {

2
Marlin/src/gcode/gcode.cpp
View file

@ -89,7 +89,7 @@ bool GcodeSuite::get_target_extruder_from_command() {
void GcodeSuite::get_destination_from_command() { void GcodeSuite::get_destination_from_command() {
LOOP_XYZE(i) { LOOP_XYZE(i) {
if (parser.seen(axis_codes[i])) if (parser.seen(axis_codes[i]))
destination[i] = parser.value_axis_units((AxisEnum)i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0); destination[i] = LOGICAL_TO_NATIVE(parser.value_axis_units((AxisEnum)i) + (axis_relative_modes[i] || relative_mode ? current_position[i] : 0), i);
else else
destination[i] = current_position[i]; destination[i] = current_position[i];
} }

2
Marlin/src/gcode/geometry/M206_M428.cpp
View file

@ -67,7 +67,7 @@ void GcodeSuite::M428() {
LOOP_XYZ(i) { LOOP_XYZ(i) {
if (axis_homed[i]) { if (axis_homed[i]) {
const float base = (current_position[i] > (soft_endstop_min[i] + soft_endstop_max[i]) * 0.5) ? base_home_pos((AxisEnum)i) : 0, const float base = (current_position[i] > (soft_endstop_min[i] + soft_endstop_max[i]) * 0.5) ? base_home_pos((AxisEnum)i) : 0,
diff = base - RAW_POSITION(current_position[i], i); diff = base - current_position[i];
if (WITHIN(diff, -20, 20)) { if (WITHIN(diff, -20, 20)) {
set_home_offset((AxisEnum)i, diff); set_home_offset((AxisEnum)i, diff);
} }

10
Marlin/src/gcode/host/M114.cpp
View file

@ -46,11 +46,15 @@
stepper.synchronize(); stepper.synchronize();
SERIAL_PROTOCOLPGM("\nLogical:"); SERIAL_PROTOCOLPGM("\nLogical:");
report_xyze(current_position); const float logical[XYZ] = {
LOGICAL_X_POSITION(current_position[X_AXIS]),
LOGICAL_Y_POSITION(current_position[Y_AXIS]),
LOGICAL_Z_POSITION(current_position[Z_AXIS])
};
report_xyze(logical);
SERIAL_PROTOCOLPGM("Raw: "); 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(current_position);
report_xyz(raw);
SERIAL_PROTOCOLPGM("Leveled:"); SERIAL_PROTOCOLPGM("Leveled:");
float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] }; float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };

14
Marlin/src/gcode/motion/G2_G3.cpp
View file

@ -44,7 +44,7 @@
* options for G2/G3 arc generation. In future these options may be GCode tunable. * options for G2/G3 arc generation. In future these options may be GCode tunable.
*/ */
void plan_arc( void plan_arc(
float logical[XYZE], // Destination position float rtarget[XYZE], // Destination position
float *offset, // Center of rotation relative to current_position float *offset, // Center of rotation relative to current_position
uint8_t clockwise // Clockwise? uint8_t clockwise // Clockwise?
) { ) {
@ -65,10 +65,10 @@ void plan_arc(
const float radius = HYPOT(r_P, r_Q), const float radius = HYPOT(r_P, r_Q),
center_P = current_position[p_axis] - r_P, center_P = current_position[p_axis] - r_P,
center_Q = current_position[q_axis] - r_Q, center_Q = current_position[q_axis] - r_Q,
rt_X = logical[p_axis] - center_P, rt_X = rtarget[p_axis] - center_P,
rt_Y = logical[q_axis] - center_Q, rt_Y = rtarget[q_axis] - center_Q,
linear_travel = logical[l_axis] - current_position[l_axis], linear_travel = rtarget[l_axis] - current_position[l_axis],
extruder_travel = logical[E_AXIS] - current_position[E_AXIS]; extruder_travel = rtarget[E_AXIS] - current_position[E_AXIS];
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y); float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
@ -76,7 +76,7 @@ void plan_arc(
if (clockwise) angular_travel -= RADIANS(360); if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 and the target is current position // Make a circle if the angular rotation is 0 and the target is current position
if (angular_travel == 0 && current_position[p_axis] == logical[p_axis] && current_position[q_axis] == logical[q_axis]) if (angular_travel == 0 && current_position[p_axis] == rtarget[p_axis] && current_position[q_axis] == rtarget[q_axis])
angular_travel = RADIANS(360); angular_travel = RADIANS(360);
const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel)); const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
@ -176,7 +176,7 @@ void plan_arc(
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
planner.buffer_line_kinematic(logical, fr_mm_s, active_extruder); planner.buffer_line_kinematic(rtarget, fr_mm_s, active_extruder);
// As far as the parser is concerned, the position is now == target. In reality the // As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position // motion control system might still be processing the action and the real tool position

2
Marlin/src/gcode/probe/G30.cpp
View file

@ -42,7 +42,7 @@ void GcodeSuite::G30() {
const float xpos = parser.linearval('X', current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER), const float xpos = parser.linearval('X', current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER),
ypos = parser.linearval('Y', current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER); ypos = parser.linearval('Y', current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER);
if (!position_is_reachable_by_probe_xy(xpos, ypos)) return; if (!position_is_reachable_by_probe(xpos, ypos)) return;
// Disable leveling so the planner won't mess with us // Disable leveling so the planner won't mess with us
#if HAS_LEVELING #if HAS_LEVELING

4
Marlin/src/gcode/scara/M360-M364.cpp
View file

@ -32,8 +32,8 @@
inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) { inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
if (IsRunning()) { if (IsRunning()) {
forward_kinematics_SCARA(delta_a, delta_b); forward_kinematics_SCARA(delta_a, delta_b);
destination[X_AXIS] = LOGICAL_X_POSITION(cartes[X_AXIS]); destination[X_AXIS] = cartes[X_AXIS];
destination[Y_AXIS] = LOGICAL_Y_POSITION(cartes[Y_AXIS]); destination[Y_AXIS] = cartes[Y_AXIS];
destination[Z_AXIS] = current_position[Z_AXIS]; destination[Z_AXIS] = current_position[Z_AXIS];
prepare_move_to_destination(); prepare_move_to_destination();
return true; return true;

37
Marlin/src/lcd/ultralcd.cpp
View file

@ -1679,7 +1679,7 @@ void kill_screen(const char* lcd_msg) {
*/ */
static int8_t bed_corner; static int8_t bed_corner;
void _lcd_goto_next_corner() { void _lcd_goto_next_corner() {
line_to_z(LOGICAL_Z_POSITION(4.0)); line_to_z(4.0);
switch (bed_corner) { switch (bed_corner) {
case 0: case 0:
current_position[X_AXIS] = X_MIN_BED + 10; current_position[X_AXIS] = X_MIN_BED + 10;
@ -1696,7 +1696,7 @@ void kill_screen(const char* lcd_msg) {
break; break;
} }
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder); planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder);
line_to_z(LOGICAL_Z_POSITION(0.0)); line_to_z(0.0);
if (++bed_corner > 3) bed_corner = 0; if (++bed_corner > 3) bed_corner = 0;
} }
@ -1742,7 +1742,7 @@ void kill_screen(const char* lcd_msg) {
// //
void _lcd_after_probing() { void _lcd_after_probing() {
#if MANUAL_PROBE_HEIGHT > 0 #if MANUAL_PROBE_HEIGHT > 0
line_to_z(LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT); line_to_z(Z_MIN_POS + MANUAL_PROBE_HEIGHT);
#endif #endif
// Display "Done" screen and wait for moves to complete // Display "Done" screen and wait for moves to complete
#if MANUAL_PROBE_HEIGHT > 0 || ENABLED(MESH_BED_LEVELING) #if MANUAL_PROBE_HEIGHT > 0 || ENABLED(MESH_BED_LEVELING)
@ -1757,13 +1757,13 @@ void kill_screen(const char* lcd_msg) {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
// Utility to go to the next mesh point // Utility to go to the next mesh point
inline void _manual_probe_goto_xy(float x, float y) { inline void _manual_probe_goto_xy(const float &rx, const float &ry) {
#if MANUAL_PROBE_HEIGHT > 0 #if MANUAL_PROBE_HEIGHT > 0
const float prev_z = current_position[Z_AXIS]; const float prev_z = current_position[Z_AXIS];
line_to_z(LOGICAL_Z_POSITION(Z_MIN_POS) + MANUAL_PROBE_HEIGHT); line_to_z(Z_MIN_POS + MANUAL_PROBE_HEIGHT);
#endif #endif
current_position[X_AXIS] = LOGICAL_X_POSITION(x); current_position[X_AXIS] = rx;
current_position[Y_AXIS] = LOGICAL_Y_POSITION(y); current_position[Y_AXIS] = ry;
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder); planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
#if MANUAL_PROBE_HEIGHT > 0 #if MANUAL_PROBE_HEIGHT > 0
line_to_z(prev_z); line_to_z(prev_z);
@ -1893,10 +1893,7 @@ void kill_screen(const char* lcd_msg) {
mbl.zigzag(manual_probe_index, px, py); mbl.zigzag(manual_probe_index, px, py);
// Controls the loop until the move is done // Controls the loop until the move is done
_manual_probe_goto_xy( _manual_probe_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
LOGICAL_X_POSITION(mbl.index_to_xpos[px]),
LOGICAL_Y_POSITION(mbl.index_to_ypos[py])
);
// After the blocking function returns, change menus // After the blocking function returns, change menus
lcd_goto_screen(_lcd_level_bed_get_z); lcd_goto_screen(_lcd_level_bed_get_z);
@ -2377,8 +2374,8 @@ void kill_screen(const char* lcd_msg) {
* UBL LCD Map Movement * UBL LCD Map Movement
*/ */
void ubl_map_move_to_xy() { void ubl_map_move_to_xy() {
current_position[X_AXIS] = LOGICAL_X_POSITION(pgm_read_float(&ubl._mesh_index_to_xpos[x_plot])); current_position[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]);
current_position[Y_AXIS] = LOGICAL_Y_POSITION(pgm_read_float(&ubl._mesh_index_to_ypos[y_plot])); current_position[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder); planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
} }
@ -2712,17 +2709,17 @@ void kill_screen(const char* lcd_msg) {
lcd_goto_screen(_lcd_calibrate_homing); lcd_goto_screen(_lcd_calibrate_homing);
} }
void _man_probe_pt(const float &lx, const float &ly) { void _man_probe_pt(const float &rx, const float &ry) {
#if HAS_LEVELING #if HAS_LEVELING
reset_bed_level(); // After calibration bed-level data is no longer valid reset_bed_level(); // After calibration bed-level data is no longer valid
#endif #endif
float z_dest = LOGICAL_Z_POSITION((Z_CLEARANCE_BETWEEN_PROBES) + (DELTA_PRINTABLE_RADIUS) / 5); float z_dest = (Z_CLEARANCE_BETWEEN_PROBES) + (DELTA_PRINTABLE_RADIUS) / 5;
line_to_z(z_dest); line_to_z(z_dest);
current_position[X_AXIS] = LOGICAL_X_POSITION(lx); current_position[X_AXIS] = rx;
current_position[Y_AXIS] = LOGICAL_Y_POSITION(ly); current_position[Y_AXIS] = ry;
line_to_current_z(); line_to_current_z();
z_dest = LOGICAL_Z_POSITION(Z_CLEARANCE_BETWEEN_PROBES); z_dest = Z_CLEARANCE_BETWEEN_PROBES;
line_to_z(z_dest); line_to_z(z_dest);
lcd_synchronize(); lcd_synchronize();
@ -2730,8 +2727,8 @@ void kill_screen(const char* lcd_msg) {
lcd_goto_screen(lcd_move_z); lcd_goto_screen(lcd_move_z);
} }
float lcd_probe_pt(const float &lx, const float &ly) { float lcd_probe_pt(const float &rx, const float &ry) {
_man_probe_pt(lx, ly); _man_probe_pt(rx, ry);
KEEPALIVE_STATE(PAUSED_FOR_USER); KEEPALIVE_STATE(PAUSED_FOR_USER);
defer_return_to_status = true; defer_return_to_status = true;
wait_for_user = true; wait_for_user = true;

2
Marlin/src/lcd/ultralcd.h
View file

@ -119,7 +119,7 @@
#endif #endif
#if ENABLED(DELTA_CALIBRATION_MENU) #if ENABLED(DELTA_CALIBRATION_MENU)
float lcd_probe_pt(const float &lx, const float &ly); float lcd_probe_pt(const float &rx, const float &ry);
#endif #endif
#else #else

6
Marlin/src/lcd/ultralcd_impl_DOGM.h
View file

@ -649,9 +649,9 @@ static void lcd_implementation_status_screen() {
// At the first page, regenerate the XYZ strings // At the first page, regenerate the XYZ strings
if (page.page == 0) { if (page.page == 0) {
strcpy(xstring, ftostr4sign(current_position[X_AXIS])); strcpy(xstring, ftostr4sign(LOGICAL_X_POSITION(current_position[X_AXIS])));
strcpy(ystring, ftostr4sign(current_position[Y_AXIS])); strcpy(ystring, ftostr4sign(LOGICAL_Y_POSITION(current_position[Y_AXIS])));
strcpy(zstring, ftostr52sp(FIXFLOAT(current_position[Z_AXIS]))); strcpy(zstring, ftostr52sp(FIXFLOAT(LOGICAL_Z_POSITION(current_position[Z_AXIS]))));
#if ENABLED(FILAMENT_LCD_DISPLAY) && DISABLED(SDSUPPORT) #if ENABLED(FILAMENT_LCD_DISPLAY) && DISABLED(SDSUPPORT)
strcpy(wstring, ftostr12ns(filament_width_meas)); strcpy(wstring, ftostr12ns(filament_width_meas));
strcpy(mstring, itostr3(100.0 * planner.volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM])); strcpy(mstring, itostr3(100.0 * planner.volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]));

16
Marlin/src/lcd/ultralcd_impl_HD44780.h
View file

@ -621,7 +621,9 @@ FORCE_INLINE void _draw_heater_status(const int8_t heater, const char prefix, co
lcd.print(itostr3(t1 + 0.5)); lcd.print(itostr3(t1 + 0.5));
lcd.write('/'); lcd.write('/');
#if HEATER_IDLE_HANDLER #if !HEATER_IDLE_HANDLER
UNUSED(blink);
#else
const bool is_idle = (!isBed ? thermalManager.is_heater_idle(heater) : const bool is_idle = (!isBed ? thermalManager.is_heater_idle(heater) :
#if HAS_TEMP_BED #if HAS_TEMP_BED
thermalManager.is_bed_idle() thermalManager.is_bed_idle()
@ -779,12 +781,12 @@ static void lcd_implementation_status_screen() {
// When everything is ok you see a constant 'X'. // When everything is ok you see a constant 'X'.
_draw_axis_label(X_AXIS, PSTR(MSG_X), blink); _draw_axis_label(X_AXIS, PSTR(MSG_X), blink);
lcd.print(ftostr4sign(current_position[X_AXIS])); lcd.print(ftostr4sign(LOGICAL_X_POSITION(current_position[X_AXIS])));
lcd.write(' '); lcd.write(' ');
_draw_axis_label(Y_AXIS, PSTR(MSG_Y), blink); _draw_axis_label(Y_AXIS, PSTR(MSG_Y), blink);
lcd.print(ftostr4sign(current_position[Y_AXIS])); lcd.print(ftostr4sign(LOGICAL_Y_POSITION(current_position[Y_AXIS])));
#endif // HOTENDS > 1 || TEMP_SENSOR_BED != 0 #endif // HOTENDS > 1 || TEMP_SENSOR_BED != 0
@ -842,11 +844,11 @@ static void lcd_implementation_status_screen() {
#if ENABLED(LCD_PROGRESS_BAR) #if ENABLED(LCD_PROGRESS_BAR)
// Draw the progress bar if the message has shown long enough
// or if there is no message set.
#if DISABLED(LCD_SET_PROGRESS_MANUALLY) #if DISABLED(LCD_SET_PROGRESS_MANUALLY)
const uint8_t progress_bar_percent = card.percentDone(); const uint8_t progress_bar_percent = card.percentDone();
#endif #endif
// Draw the progress bar if the message has shown long enough
// or if there is no message set.
if (progress_bar_percent > 2 && (ELAPSED(millis(), progress_bar_ms + PROGRESS_BAR_MSG_TIME) || !lcd_status_message[0])) if (progress_bar_percent > 2 && (ELAPSED(millis(), progress_bar_ms + PROGRESS_BAR_MSG_TIME) || !lcd_status_message[0]))
return lcd_draw_progress_bar(progress_bar_percent); return lcd_draw_progress_bar(progress_bar_percent);
@ -1168,9 +1170,9 @@ static void lcd_implementation_status_screen() {
return ret_val; return ret_val;
} }
coordinate pixel_location(uint8_t x, uint8_t y) { return pixel_location((int16_t)x, (int16_t)y); } inline coordinate pixel_location(const uint8_t x, const uint8_t y) { return pixel_location((int16_t)x, (int16_t)y); }
void lcd_implementation_ubl_plot(uint8_t x, uint8_t inverted_y) { void lcd_implementation_ubl_plot(const uint8_t x, const uint8_t inverted_y) {
#if LCD_WIDTH >= 20 #if LCD_WIDTH >= 20
#define _LCD_W_POS 12 #define _LCD_W_POS 12

14
Marlin/src/module/delta.cpp
View file

@ -72,7 +72,7 @@ void recalc_delta_settings(const float radius, const float diagonal_rod, const f
/** /**
* Delta Inverse Kinematics * Delta Inverse Kinematics
* *
* Calculate the tower positions for a given logical * Calculate the tower positions for a given machine
* position, storing the result in the delta[] array. * position, storing the result in the delta[] array.
* *
* This is an expensive calculation, requiring 3 square * This is an expensive calculation, requiring 3 square
@ -117,8 +117,8 @@ void recalc_delta_settings(const float radius, const float diagonal_rod, const f
SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \ SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \
}while(0) }while(0)
void inverse_kinematics(const float logical[XYZ]) { void inverse_kinematics(const float raw[XYZ]) {
DELTA_LOGICAL_IK(); DELTA_RAW_IK();
// DELTA_DEBUG(); // DELTA_DEBUG();
} }
@ -127,14 +127,10 @@ void inverse_kinematics(const float logical[XYZ]) {
* effector has the full range of XY motion. * effector has the full range of XY motion.
*/ */
float delta_safe_distance_from_top() { float delta_safe_distance_from_top() {
float cartesian[XYZ] = { float cartesian[XYZ] = { 0, 0, 0 };
LOGICAL_X_POSITION(0),
LOGICAL_Y_POSITION(0),
LOGICAL_Z_POSITION(0)
};
inverse_kinematics(cartesian); inverse_kinematics(cartesian);
float distance = delta[A_AXIS]; float distance = delta[A_AXIS];
cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS); cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
inverse_kinematics(cartesian); inverse_kinematics(cartesian);
return FABS(distance - delta[A_AXIS]); return FABS(distance - delta[A_AXIS]);
} }

13
Marlin/src/module/delta.h
View file

@ -47,7 +47,7 @@ void recalc_delta_settings(const float radius, const float diagonal_rod, const f
/** /**
* Delta Inverse Kinematics * Delta Inverse Kinematics
* *
* Calculate the tower positions for a given logical * Calculate the tower positions for a given machine
* position, storing the result in the delta[] array. * position, storing the result in the delta[] array.
* *
* This is an expensive calculation, requiring 3 square * This is an expensive calculation, requiring 3 square
@ -88,16 +88,7 @@ void recalc_delta_settings(const float radius, const float diagonal_rod, const f
delta[C_AXIS] = DELTA_Z(C_AXIS); \ delta[C_AXIS] = DELTA_Z(C_AXIS); \
}while(0) }while(0)
#define DELTA_LOGICAL_IK() do { \ void inverse_kinematics(const float raw[XYZ]);
const float raw[XYZ] = { \
RAW_X_POSITION(logical[X_AXIS]), \
RAW_Y_POSITION(logical[Y_AXIS]), \
RAW_Z_POSITION(logical[Z_AXIS]) \
}; \
DELTA_RAW_IK(); \
}while(0)
void inverse_kinematics(const float logical[XYZ]);
/** /**
* Calculate the highest Z position where the * Calculate the highest Z position where the

128
Marlin/src/module/motion.cpp
View file

@ -73,7 +73,7 @@ bool relative_mode = false;
/** /**
* Cartesian Current Position * Cartesian Current Position
* Used to track the logical position as moves are queued. * Used to track the native machine position as moves are queued.
* Used by 'line_to_current_position' to do a move after changing it. * Used by 'line_to_current_position' to do a move after changing it.
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'. * Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
*/ */
@ -197,20 +197,16 @@ void get_cartesian_from_steppers() {
stepper.get_axis_position_mm(B_AXIS), stepper.get_axis_position_mm(B_AXIS),
stepper.get_axis_position_mm(C_AXIS) stepper.get_axis_position_mm(C_AXIS)
); );
cartes[X_AXIS] += LOGICAL_X_POSITION(0); #else
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0); #if IS_SCARA
cartes[Z_AXIS] += LOGICAL_Z_POSITION(0);
#elif IS_SCARA
forward_kinematics_SCARA( forward_kinematics_SCARA(
stepper.get_axis_position_degrees(A_AXIS), stepper.get_axis_position_degrees(A_AXIS),
stepper.get_axis_position_degrees(B_AXIS) stepper.get_axis_position_degrees(B_AXIS)
); );
cartes[X_AXIS] += LOGICAL_X_POSITION(0);
cartes[Y_AXIS] += LOGICAL_Y_POSITION(0);
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#else #else
cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); cartes[X_AXIS] = stepper.get_axis_position_mm(X_AXIS);
cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS); cartes[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
#endif
cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS); cartes[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
#endif #endif
} }
@ -288,16 +284,16 @@ void line_to_destination(const float fr_mm_s) {
* Plan a move to (X, Y, Z) and set the current_position * Plan a move to (X, Y, Z) and set the current_position
* The final current_position may not be the one that was requested * The final current_position may not be the one that was requested
*/ */
void do_blocking_move_to(const float &lx, const float &ly, const float &lz, const float &fr_mm_s/*=0.0*/) { void do_blocking_move_to(const float &rx, const float &ry, const float &rz, const float &fr_mm_s/*=0.0*/) {
const float old_feedrate_mm_s = feedrate_mm_s; const float old_feedrate_mm_s = feedrate_mm_s;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, lx, ly, lz); if (DEBUGGING(LEVELING)) print_xyz(PSTR(">>> do_blocking_move_to"), NULL, rx, ry, rz);
#endif #endif
#if ENABLED(DELTA) #if ENABLED(DELTA)
if (!position_is_reachable_xy(lx, ly)) return; if (!position_is_reachable(rx, ry)) return;
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S; feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
@ -309,10 +305,10 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
// when in the danger zone // when in the danger zone
if (current_position[Z_AXIS] > delta_clip_start_height) { if (current_position[Z_AXIS] > delta_clip_start_height) {
if (lz > delta_clip_start_height) { // staying in the danger zone if (rz > delta_clip_start_height) { // staying in the danger zone
destination[X_AXIS] = lx; // move directly (uninterpolated) destination[X_AXIS] = rx; // move directly (uninterpolated)
destination[Y_AXIS] = ly; destination[Y_AXIS] = ry;
destination[Z_AXIS] = lz; destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination() prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("danger zone move", current_position);
@ -328,23 +324,23 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
} }
} }
if (lz > current_position[Z_AXIS]) { // raising? if (rz > current_position[Z_AXIS]) { // raising?
destination[Z_AXIS] = lz; destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination() prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("z raise move", current_position);
#endif #endif
} }
destination[X_AXIS] = lx; destination[X_AXIS] = rx;
destination[Y_AXIS] = ly; destination[Y_AXIS] = ry;
prepare_move_to_destination(); // set_current_from_destination() prepare_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("xy move", current_position);
#endif #endif
if (lz < current_position[Z_AXIS]) { // lowering? if (rz < current_position[Z_AXIS]) { // lowering?
destination[Z_AXIS] = lz; destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(); // set_current_from_destination() prepare_uninterpolated_move_to_destination(); // set_current_from_destination()
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("z lower move", current_position);
@ -353,44 +349,44 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
#elif IS_SCARA #elif IS_SCARA
if (!position_is_reachable_xy(lx, ly)) return; if (!position_is_reachable(rx, ry)) return;
set_destination_from_current(); set_destination_from_current();
// If Z needs to raise, do it before moving XY // If Z needs to raise, do it before moving XY
if (destination[Z_AXIS] < lz) { if (destination[Z_AXIS] < rz) {
destination[Z_AXIS] = lz; destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS)); prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
} }
destination[X_AXIS] = lx; destination[X_AXIS] = rx;
destination[Y_AXIS] = ly; destination[Y_AXIS] = ry;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S); prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S);
// If Z needs to lower, do it after moving XY // If Z needs to lower, do it after moving XY
if (destination[Z_AXIS] > lz) { if (destination[Z_AXIS] > rz) {
destination[Z_AXIS] = lz; destination[Z_AXIS] = rz;
prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS)); prepare_uninterpolated_move_to_destination(fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS));
} }
#else #else
// If Z needs to raise, do it before moving XY // If Z needs to raise, do it before moving XY
if (current_position[Z_AXIS] < lz) { if (current_position[Z_AXIS] < rz) {
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS); feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
current_position[Z_AXIS] = lz; current_position[Z_AXIS] = rz;
line_to_current_position(); line_to_current_position();
} }
feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S; feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
current_position[X_AXIS] = lx; current_position[X_AXIS] = rx;
current_position[Y_AXIS] = ly; current_position[Y_AXIS] = ry;
line_to_current_position(); line_to_current_position();
// If Z needs to lower, do it after moving XY // If Z needs to lower, do it after moving XY
if (current_position[Z_AXIS] > lz) { if (current_position[Z_AXIS] > rz) {
feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS); feedrate_mm_s = fr_mm_s ? fr_mm_s : homing_feedrate(Z_AXIS);
current_position[Z_AXIS] = lz; current_position[Z_AXIS] = rz;
line_to_current_position(); line_to_current_position();
} }
@ -404,14 +400,14 @@ void do_blocking_move_to(const float &lx, const float &ly, const float &lz, cons
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to"); if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< do_blocking_move_to");
#endif #endif
} }
void do_blocking_move_to_x(const float &lx, const float &fr_mm_s/*=0.0*/) { void do_blocking_move_to_x(const float &rx, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(lx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s); do_blocking_move_to(rx, current_position[Y_AXIS], current_position[Z_AXIS], fr_mm_s);
} }
void do_blocking_move_to_z(const float &lz, const float &fr_mm_s/*=0.0*/) { void do_blocking_move_to_z(const float &rz, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], lz, fr_mm_s); do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], rz, fr_mm_s);
} }
void do_blocking_move_to_xy(const float &lx, const float &ly, const float &fr_mm_s/*=0.0*/) { void do_blocking_move_to_xy(const float &rx, const float &ry, const float &fr_mm_s/*=0.0*/) {
do_blocking_move_to(lx, ly, current_position[Z_AXIS], fr_mm_s); do_blocking_move_to(rx, ry, current_position[Z_AXIS], fr_mm_s);
} }
// //
@ -521,26 +517,26 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
* This calls planner.buffer_line several times, adding * This calls planner.buffer_line several times, adding
* small incremental moves for DELTA or SCARA. * small incremental moves for DELTA or SCARA.
*/ */
inline bool prepare_kinematic_move_to(float ltarget[XYZE]) { inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
// Get the top feedrate of the move in the XY plane // Get the top feedrate of the move in the XY plane
const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s); const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
// If the move is only in Z/E don't split up the move // If the move is only in Z/E don't split up the move
if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) { if (rtarget[X_AXIS] == current_position[X_AXIS] && rtarget[Y_AXIS] == current_position[Y_AXIS]) {
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder); planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
return false; return false;
} }
// Fail if attempting move outside printable radius // Fail if attempting move outside printable radius
if (!position_is_reachable_xy(ltarget[X_AXIS], ltarget[Y_AXIS])) return true; if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true;
// Get the cartesian distances moved in XYZE // Get the cartesian distances moved in XYZE
const float difference[XYZE] = { const float difference[XYZE] = {
ltarget[X_AXIS] - current_position[X_AXIS], rtarget[X_AXIS] - current_position[X_AXIS],
ltarget[Y_AXIS] - current_position[Y_AXIS], rtarget[Y_AXIS] - current_position[Y_AXIS],
ltarget[Z_AXIS] - current_position[Z_AXIS], rtarget[Z_AXIS] - current_position[Z_AXIS],
ltarget[E_AXIS] - current_position[E_AXIS] rtarget[E_AXIS] - current_position[E_AXIS]
}; };
// Get the linear distance in XYZ // Get the linear distance in XYZ
@ -588,9 +584,9 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
oldB = stepper.get_axis_position_degrees(B_AXIS); oldB = stepper.get_axis_position_degrees(B_AXIS);
#endif #endif
// Get the logical current position as starting point // Get the current position as starting point
float logical[XYZE]; float raw[XYZE];
COPY(logical, current_position); COPY(raw, current_position);
// Drop one segment so the last move is to the exact target. // Drop one segment so the last move is to the exact target.
// If there's only 1 segment, loops will be skipped entirely. // If there's only 1 segment, loops will be skipped entirely.
@ -598,25 +594,25 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// Calculate and execute the segments // Calculate and execute the segments
for (uint16_t s = segments + 1; --s;) { for (uint16_t s = segments + 1; --s;) {
LOOP_XYZE(i) logical[i] += segment_distance[i]; LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA) #if ENABLED(DELTA)
DELTA_LOGICAL_IK(); // Delta can inline its kinematics DELTA_RAW_IK(); // Delta can inline its kinematics
#else #else
inverse_kinematics(logical); inverse_kinematics(raw);
#endif #endif
ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING) #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s // For SCARA scale the feed rate from mm/s to degrees/s
// Use ratio between the length of the move and the larger angle change // Use ratio between the length of the move and the larger angle change
const float adiff = abs(delta[A_AXIS] - oldA), const float adiff = abs(delta[A_AXIS] - oldA),
bdiff = abs(delta[B_AXIS] - oldB); bdiff = abs(delta[B_AXIS] - oldB);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
oldA = delta[A_AXIS]; oldA = delta[A_AXIS];
oldB = delta[B_AXIS]; oldB = delta[B_AXIS];
#else #else
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder);
#endif #endif
} }
@ -626,13 +622,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING) #if IS_SCARA && ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s // For SCARA scale the feed rate from mm/s to degrees/s
// With segments > 1 length is 1 segment, otherwise total length // With segments > 1 length is 1 segment, otherwise total length
inverse_kinematics(ltarget); inverse_kinematics(rtarget);
ADJUST_DELTA(ltarget); ADJUST_DELTA(rtarget);
const float adiff = abs(delta[A_AXIS] - oldA), const float adiff = abs(delta[A_AXIS] - oldA),
bdiff = abs(delta[B_AXIS] - oldB); bdiff = abs(delta[B_AXIS] - oldB);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder);
#else #else
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder); planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
#endif #endif
return false; return false;
@ -687,7 +683,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
float x_home_pos(const int extruder) { float x_home_pos(const int extruder) {
if (extruder == 0) if (extruder == 0)
return LOGICAL_X_POSITION(base_home_pos(X_AXIS)); return base_home_pos(X_AXIS);
else else
/** /**
* In dual carriage mode the extruder offset provides an override of the * In dual carriage mode the extruder offset provides an override of the
@ -695,7 +691,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
* This allows soft recalibration of the second extruder home position * This allows soft recalibration of the second extruder home position
* without firmware reflash (through the M218 command). * without firmware reflash (through the M218 command).
*/ */
return LOGICAL_X_POSITION(hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS); return hotend_offset[X_AXIS][1] > 0 ? hotend_offset[X_AXIS][1] : X2_HOME_POS;
} }
/** /**
@ -740,13 +736,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
if (active_extruder == 0) { if (active_extruder == 0) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("Set planner X", LOGICAL_X_POSITION(inactive_extruder_x_pos)); SERIAL_ECHOPAIR("Set planner X", inactive_extruder_x_pos);
SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset); SERIAL_ECHOLNPAIR(" ... Line to X", current_position[X_AXIS] + duplicate_extruder_x_offset);
} }
#endif #endif
// move duplicate extruder into correct duplication position. // move duplicate extruder into correct duplication position.
planner.set_position_mm( planner.set_position_mm(
LOGICAL_X_POSITION(inactive_extruder_x_pos), inactive_extruder_x_pos,
current_position[Y_AXIS], current_position[Y_AXIS],
current_position[Z_AXIS], current_position[Z_AXIS],
current_position[E_AXIS] current_position[E_AXIS]
@ -970,7 +966,7 @@ void set_axis_is_at_home(const AxisEnum axis) {
#if ENABLED(MORGAN_SCARA) #if ENABLED(MORGAN_SCARA)
scara_set_axis_is_at_home(axis); scara_set_axis_is_at_home(axis);
#else #else
current_position[axis] = LOGICAL_POSITION(base_home_pos(axis), axis); current_position[axis] = base_home_pos(axis);
#endif #endif
/** /**

37
Marlin/src/module/motion.h
View file

@ -217,14 +217,14 @@ void homeaxis(const AxisEnum axis);
#define WORKSPACE_OFFSET(AXIS) 0 #define WORKSPACE_OFFSET(AXIS) 0
#endif #endif
#define LOGICAL_POSITION(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS)) #define NATIVE_TO_LOGICAL(POS, AXIS) ((POS) + WORKSPACE_OFFSET(AXIS))
#define RAW_POSITION(POS, AXIS) ((POS) - WORKSPACE_OFFSET(AXIS)) #define LOGICAL_TO_NATIVE(POS, AXIS) ((POS) - WORKSPACE_OFFSET(AXIS))
#if HAS_POSITION_SHIFT || DISABLED(DELTA) #if HAS_POSITION_SHIFT || DISABLED(DELTA)
#define LOGICAL_X_POSITION(POS) LOGICAL_POSITION(POS, X_AXIS) #define LOGICAL_X_POSITION(POS) NATIVE_TO_LOGICAL(POS, X_AXIS)
#define LOGICAL_Y_POSITION(POS) LOGICAL_POSITION(POS, Y_AXIS) #define LOGICAL_Y_POSITION(POS) NATIVE_TO_LOGICAL(POS, Y_AXIS)
#define RAW_X_POSITION(POS) RAW_POSITION(POS, X_AXIS) #define RAW_X_POSITION(POS) LOGICAL_TO_NATIVE(POS, X_AXIS)
#define RAW_Y_POSITION(POS) RAW_POSITION(POS, Y_AXIS) #define RAW_Y_POSITION(POS) LOGICAL_TO_NATIVE(POS, Y_AXIS)
#else #else
#define LOGICAL_X_POSITION(POS) (POS) #define LOGICAL_X_POSITION(POS) (POS)
#define LOGICAL_Y_POSITION(POS) (POS) #define LOGICAL_Y_POSITION(POS) (POS)
@ -232,9 +232,8 @@ void homeaxis(const AxisEnum axis);
#define RAW_Y_POSITION(POS) (POS) #define RAW_Y_POSITION(POS) (POS)
#endif #endif
#define LOGICAL_Z_POSITION(POS) LOGICAL_POSITION(POS, Z_AXIS) #define LOGICAL_Z_POSITION(POS) NATIVE_TO_LOGICAL(POS, Z_AXIS)
#define RAW_Z_POSITION(POS) RAW_POSITION(POS, Z_AXIS) #define RAW_Z_POSITION(POS) LOGICAL_TO_NATIVE(POS, Z_AXIS)
#define RAW_CURRENT_POSITION(A) RAW_##A##_POSITION(current_position[A##_AXIS])
/** /**
* position_is_reachable family of functions * position_is_reachable family of functions
@ -242,7 +241,7 @@ void homeaxis(const AxisEnum axis);
#if IS_KINEMATIC // (DELTA or SCARA) #if IS_KINEMATIC // (DELTA or SCARA)
inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) { inline bool position_is_reachable(const float &rx, const float &ry) {
#if ENABLED(DELTA) #if ENABLED(DELTA)
return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS); return HYPOT2(rx, ry) <= sq(DELTA_PRINTABLE_RADIUS);
#elif IS_SCARA #elif IS_SCARA
@ -257,24 +256,24 @@ void homeaxis(const AxisEnum axis);
#endif #endif
} }
inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) { inline bool position_is_reachable_by_probe(const float &rx, const float &ry) {
// Both the nozzle and the probe must be able to reach the point. // Both the nozzle and the probe must be able to reach the point.
// This won't work on SCARA since the probe offset rotates with the arm. // This won't work on SCARA since the probe offset rotates with the arm.
return position_is_reachable_raw_xy(rx, ry) return position_is_reachable(rx, ry)
&& position_is_reachable_raw_xy(rx - X_PROBE_OFFSET_FROM_EXTRUDER, ry - Y_PROBE_OFFSET_FROM_EXTRUDER); && position_is_reachable(rx - X_PROBE_OFFSET_FROM_EXTRUDER, ry - Y_PROBE_OFFSET_FROM_EXTRUDER);
} }
#else // CARTESIAN #else // CARTESIAN
inline bool position_is_reachable_raw_xy(const float &rx, const float &ry) { inline bool position_is_reachable(const float &rx, const float &ry) {
// Add 0.001 margin to deal with float imprecision // Add 0.001 margin to deal with float imprecision
return WITHIN(rx, X_MIN_POS - 0.001, X_MAX_POS + 0.001) return WITHIN(rx, X_MIN_POS - 0.001, X_MAX_POS + 0.001)
&& WITHIN(ry, Y_MIN_POS - 0.001, Y_MAX_POS + 0.001); && WITHIN(ry, Y_MIN_POS - 0.001, Y_MAX_POS + 0.001);
} }
inline bool position_is_reachable_by_probe_raw_xy(const float &rx, const float &ry) { inline bool position_is_reachable_by_probe(const float &rx, const float &ry) {
// Add 0.001 margin to deal with float imprecision // Add 0.001 margin to deal with float imprecision
return WITHIN(rx, MIN_PROBE_X - 0.001, MAX_PROBE_X + 0.001) return WITHIN(rx, MIN_PROBE_X - 0.001, MAX_PROBE_X + 0.001)
&& WITHIN(ry, MIN_PROBE_Y - 0.001, MAX_PROBE_Y + 0.001); && WITHIN(ry, MIN_PROBE_Y - 0.001, MAX_PROBE_Y + 0.001);
@ -282,14 +281,6 @@ void homeaxis(const AxisEnum axis);
#endif // CARTESIAN #endif // CARTESIAN
FORCE_INLINE bool position_is_reachable_by_probe_xy(const float &lx, const float &ly) {
return position_is_reachable_by_probe_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
}
FORCE_INLINE bool position_is_reachable_xy(const float &lx, const float &ly) {
return position_is_reachable_raw_xy(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
}
/** /**
* Dual X Carriage / Dual Nozzle * Dual X Carriage / Dual Nozzle
*/ */

72
Marlin/src/module/planner.cpp
View file

@ -132,7 +132,7 @@ float Planner::min_feedrate_mm_s,
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
float Planner::z_fade_height, // Initialized by settings.load() float Planner::z_fade_height, // Initialized by settings.load()
Planner::inverse_z_fade_height, Planner::inverse_z_fade_height,
Planner::last_raw_lz; Planner::last_fade_z;
#endif #endif
#if ENABLED(AUTOTEMP) #if ENABLED(AUTOTEMP)
@ -552,14 +552,14 @@ void Planner::calculate_volumetric_multipliers() {
#if PLANNER_LEVELING #if PLANNER_LEVELING
/** /**
* lx, ly, lz - logical (cartesian, not delta) positions in mm * rx, ry, rz - Cartesian positions in mm
*/ */
void Planner::apply_leveling(float &lx, float &ly, float &lz) { void Planner::apply_leveling(float &rx, float &ry, float &rz) {
if (!planner.leveling_active) return; if (!planner.leveling_active) return;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = fade_scaling_factor_for_z(lz); const float fade_scaling_factor = fade_scaling_factor_for_z(rz);
if (!fade_scaling_factor) return; if (!fade_scaling_factor) return;
#else #else
constexpr float fade_scaling_factor = 1.0; constexpr float fade_scaling_factor = 1.0;
@ -567,11 +567,11 @@ void Planner::calculate_volumetric_multipliers() {
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
lz += ubl.get_z_correction(lx, ly) * fade_scaling_factor; rz += ubl.get_z_correction(rx, ry) * fade_scaling_factor;
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)
lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly) rz += mbl.get_z(rx, ry
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, fade_scaling_factor , fade_scaling_factor
#endif #endif
@ -581,42 +581,38 @@ void Planner::calculate_volumetric_multipliers() {
UNUSED(fade_scaling_factor); UNUSED(fade_scaling_factor);
float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM), float dx = rx - (X_TILT_FULCRUM),
dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM), dy = ry - (Y_TILT_FULCRUM);
dz = RAW_Z_POSITION(lz);
apply_rotation_xyz(bed_level_matrix, dx, dy, dz); apply_rotation_xyz(bed_level_matrix, dx, dy, rz);
lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM); rx = dx + X_TILT_FULCRUM;
ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); ry = dy + Y_TILT_FULCRUM;
lz = LOGICAL_Z_POSITION(dz);
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
float tmp[XYZ] = { lx, ly, 0 }; float tmp[XYZ] = { rx, ry, 0 };
lz += bilinear_z_offset(tmp) * fade_scaling_factor; rz += bilinear_z_offset(tmp) * fade_scaling_factor;
#endif #endif
} }
void Planner::unapply_leveling(float logical[XYZ]) { void Planner::unapply_leveling(float raw[XYZ]) {
if (!planner.leveling_active) return; if (!planner.leveling_active) return;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (z_fade_height && RAW_Z_POSITION(logical[Z_AXIS]) >= z_fade_height) return; if (z_fade_height && raw[Z_AXIS] >= z_fade_height) return;
#endif #endif
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
const float z_physical = RAW_Z_POSITION(logical[Z_AXIS]), const float z_correct = ubl.get_z_correction(raw[X_AXIS], raw[Y_AXIS]);
z_correct = ubl.get_z_correction(logical[X_AXIS], logical[Y_AXIS]), float z_raw = raw[Z_AXIS] - z_correct;
z_virtual = z_physical - z_correct;
float z_logical = LOGICAL_Z_POSITION(z_virtual);
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
// for P=physical_z, L=logical_z, M=mesh_z, H=fade_height, // for P=physical_z, L=raw_z, M=mesh_z, H=fade_height,
// Given P=L+M(1-L/H) (faded mesh correction formula for L<H) // Given P=L+M(1-L/H) (faded mesh correction formula for L<H)
// then L=P-M(1-L/H) // then L=P-M(1-L/H)
// so L=P-M+ML/H // so L=P-M+ML/H
@ -625,46 +621,46 @@ void Planner::calculate_volumetric_multipliers() {
// so L=(P-M)/(1-M/H) for L<H // so L=(P-M)/(1-M/H) for L<H
if (planner.z_fade_height) { if (planner.z_fade_height) {
if (z_logical >= planner.z_fade_height) if (z_raw >= planner.z_fade_height)
z_logical = LOGICAL_Z_POSITION(z_physical); z_raw = raw[Z_AXIS];
else else
z_logical /= 1.0 - z_correct * planner.inverse_z_fade_height; z_raw /= 1.0 - z_correct * planner.inverse_z_fade_height;
} }
#endif // ENABLE_LEVELING_FADE_HEIGHT #endif // ENABLE_LEVELING_FADE_HEIGHT
logical[Z_AXIS] = z_logical; raw[Z_AXIS] = z_raw;
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float c = mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]), 1.0); const float c = mbl.get_z(raw[X_AXIS], raw[Y_AXIS], 1.0);
logical[Z_AXIS] = (z_fade_height * (RAW_Z_POSITION(logical[Z_AXIS]) - c)) / (z_fade_height - c); raw[Z_AXIS] = (z_fade_height * (raw[Z_AXIS] - c)) / (z_fade_height - c);
#else #else
logical[Z_AXIS] -= mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS])); raw[Z_AXIS] -= mbl.get_z(raw[X_AXIS], raw[Y_AXIS]);
#endif #endif
#elif ABL_PLANAR #elif ABL_PLANAR
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix); matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
float dx = RAW_X_POSITION(logical[X_AXIS]) - (X_TILT_FULCRUM), float dx = raw[X_AXIS] - (X_TILT_FULCRUM),
dy = RAW_Y_POSITION(logical[Y_AXIS]) - (Y_TILT_FULCRUM), dy = raw[Y_AXIS] - (Y_TILT_FULCRUM),
dz = RAW_Z_POSITION(logical[Z_AXIS]); dz = raw[Z_AXIS];
apply_rotation_xyz(inverse, dx, dy, dz); apply_rotation_xyz(inverse, dx, dy, dz);
logical[X_AXIS] = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM); raw[X_AXIS] = dx + X_TILT_FULCRUM;
logical[Y_AXIS] = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); raw[Y_AXIS] = dy + Y_TILT_FULCRUM;
logical[Z_AXIS] = LOGICAL_Z_POSITION(dz); raw[Z_AXIS] = dz;
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float c = bilinear_z_offset(logical); const float c = bilinear_z_offset(raw);
logical[Z_AXIS] = (z_fade_height * (RAW_Z_POSITION(logical[Z_AXIS]) - c)) / (z_fade_height - c); raw[Z_AXIS] = (z_fade_height * (raw[Z_AXIS]) - c) / (z_fade_height - c);
#else #else
logical[Z_AXIS] -= bilinear_z_offset(logical); raw[Z_AXIS] -= bilinear_z_offset(raw);
#endif #endif
#endif #endif

65
Marlin/src/module/planner.h
View file

@ -202,7 +202,7 @@ class Planner {
static uint32_t cutoff_long; static uint32_t cutoff_long;
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
static float last_raw_lz; static float last_fade_z;
#endif #endif
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) #if ENABLED(DISABLE_INACTIVE_EXTRUDER)
@ -275,21 +275,20 @@ class Planner {
* Returns 1.0 if planner.z_fade_height is 0.0. * Returns 1.0 if planner.z_fade_height is 0.0.
* Returns 0.0 if Z is past the specified 'Fade Height'. * Returns 0.0 if Z is past the specified 'Fade Height'.
*/ */
inline static float fade_scaling_factor_for_z(const float &lz) { inline static float fade_scaling_factor_for_z(const float &rz) {
static float z_fade_factor = 1.0; static float z_fade_factor = 1.0;
if (z_fade_height) { if (z_fade_height) {
const float raw_lz = RAW_Z_POSITION(lz); if (rz >= z_fade_height) return 0.0;
if (raw_lz >= z_fade_height) return 0.0; if (last_fade_z != rz) {
if (last_raw_lz != raw_lz) { last_fade_z = rz;
last_raw_lz = raw_lz; z_fade_factor = 1.0 - rz * inverse_z_fade_height;
z_fade_factor = 1.0 - raw_lz * inverse_z_fade_height;
} }
return z_fade_factor; return z_fade_factor;
} }
return 1.0; return 1.0;
} }
FORCE_INLINE static void force_fade_recalc() { last_raw_lz = -999.999; } FORCE_INLINE static void force_fade_recalc() { last_fade_z = -999.999; }
FORCE_INLINE static void set_z_fade_height(const float &zfh) { FORCE_INLINE static void set_z_fade_height(const float &zfh) {
z_fade_height = zfh > 0 ? zfh : 0; z_fade_height = zfh > 0 ? zfh : 0;
@ -297,40 +296,40 @@ class Planner {
force_fade_recalc(); force_fade_recalc();
} }
FORCE_INLINE static bool leveling_active_at_z(const float &lz) { FORCE_INLINE static bool leveling_active_at_z(const float &rz) {
return !z_fade_height || RAW_Z_POSITION(lz) < z_fade_height; return !z_fade_height || rz < z_fade_height;
} }
#else #else
FORCE_INLINE static float fade_scaling_factor_for_z(const float &lz) { FORCE_INLINE static float fade_scaling_factor_for_z(const float &rz) {
UNUSED(lz); UNUSED(rz);
return 1.0; return 1.0;
} }
FORCE_INLINE static bool leveling_active_at_z(const float &lz) { UNUSED(lz); return true; } FORCE_INLINE static bool leveling_active_at_z(const float &rz) { UNUSED(rz); return true; }
#endif #endif
#if PLANNER_LEVELING #if PLANNER_LEVELING
#define ARG_X float lx #define ARG_X float rx
#define ARG_Y float ly #define ARG_Y float ry
#define ARG_Z float lz #define ARG_Z float rz
/** /**
* Apply leveling to transform a cartesian position * Apply leveling to transform a cartesian position
* as it will be given to the planner and steppers. * as it will be given to the planner and steppers.
*/ */
static void apply_leveling(float &lx, float &ly, float &lz); static void apply_leveling(float &rx, float &ry, float &rz);
static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); } static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
static void unapply_leveling(float logical[XYZ]); static void unapply_leveling(float raw[XYZ]);
#else #else
#define ARG_X const float &lx #define ARG_X const float &rx
#define ARG_Y const float &ly #define ARG_Y const float &ry
#define ARG_Z const float &lz #define ARG_Z const float &rz
#endif #endif
@ -357,15 +356,15 @@ class Planner {
* Kinematic machines should call buffer_line_kinematic (for leveled moves). * Kinematic machines should call buffer_line_kinematic (for leveled moves).
* (Cartesians may also call buffer_line_kinematic.) * (Cartesians may also call buffer_line_kinematic.)
* *
* lx,ly,lz,e - target position in mm or degrees * rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - target extruder
*/ */
static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) { static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING && IS_CARTESIAN #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz); apply_leveling(rx, ry, rz);
#endif #endif
_buffer_line(lx, ly, lz, e, fr_mm_s, extruder); _buffer_line(rx, ry, rz, e, fr_mm_s, extruder);
} }
/** /**
@ -373,22 +372,22 @@ class Planner {
* The target is cartesian, it's translated to delta/scara if * The target is cartesian, it's translated to delta/scara if
* needed. * needed.
* *
* ltarget - x,y,z,e CARTESIAN target in mm * rtarget - x,y,z,e CARTESIAN target in mm
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - target extruder
*/ */
static FORCE_INLINE void buffer_line_kinematic(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) { static FORCE_INLINE void buffer_line_kinematic(const float rtarget[XYZE], const float &fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING #if PLANNER_LEVELING
float lpos[XYZ] = { ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS] }; float lpos[XYZ] = { rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS] };
apply_leveling(lpos); apply_leveling(lpos);
#else #else
const float * const lpos = ltarget; const float * const lpos = rtarget;
#endif #endif
#if IS_KINEMATIC #if IS_KINEMATIC
inverse_kinematics(lpos); inverse_kinematics(lpos);
_buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], fr_mm_s, extruder); _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], rtarget[E_AXIS], fr_mm_s, extruder);
#else #else
_buffer_line(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder); _buffer_line(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], rtarget[E_AXIS], fr_mm_s, extruder);
#endif #endif
} }
@ -403,9 +402,9 @@ class Planner {
*/ */
static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING && IS_CARTESIAN #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz); apply_leveling(rx, ry, rz);
#endif #endif
_set_position_mm(lx, ly, lz, e); _set_position_mm(rx, ry, rz, e);
} }
static void set_position_mm_kinematic(const float position[NUM_AXIS]); static void set_position_mm_kinematic(const float position[NUM_AXIS]);
static void set_position_mm(const AxisEnum axis, const float &v); static void set_position_mm(const AxisEnum axis, const float &v);

22
Marlin/src/module/probe.cpp
View file

@ -106,8 +106,8 @@ inline void do_probe_raise(const float z_raise) {
#elif ENABLED(Z_PROBE_ALLEN_KEY) #elif ENABLED(Z_PROBE_ALLEN_KEY)
FORCE_INLINE void do_blocking_move_to(const float logical[XYZ], const float &fr_mm_s) { FORCE_INLINE void do_blocking_move_to(const float raw[XYZ], const float &fr_mm_s) {
do_blocking_move_to(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], fr_mm_s); do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
} }
void run_deploy_moves_script() { void run_deploy_moves_script() {
@ -564,7 +564,7 @@ static float run_z_probe(const bool short_move=true) {
} }
#endif #endif
return RAW_CURRENT_POSITION(Z) + zprobe_zoffset return current_position[Z_AXIS] + zprobe_zoffset
#if ENABLED(DELTA) #if ENABLED(DELTA)
+ home_offset[Z_AXIS] // Account for delta height adjustment + home_offset[Z_AXIS] // Account for delta height adjustment
#endif #endif
@ -580,22 +580,22 @@ static float run_z_probe(const bool short_move=true) {
* - Raise to the BETWEEN height * - Raise to the BETWEEN height
* - Return the probed Z position * - Return the probed Z position
*/ */
float probe_pt(const float &lx, const float &ly, const bool stow, const uint8_t verbose_level, const bool printable/*=true*/) { float probe_pt(const float &rx, const float &ry, const bool stow, const uint8_t verbose_level, const bool printable/*=true*/) {
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR(">>> probe_pt(", lx); SERIAL_ECHOPAIR(">>> probe_pt(", LOGICAL_X_POSITION(rx));
SERIAL_ECHOPAIR(", ", ly); SERIAL_ECHOPAIR(", ", LOGICAL_Y_POSITION(ry));
SERIAL_ECHOPAIR(", ", stow ? "" : "no "); SERIAL_ECHOPAIR(", ", stow ? "" : "no ");
SERIAL_ECHOLNPGM("stow)"); SERIAL_ECHOLNPGM("stow)");
DEBUG_POS("", current_position); DEBUG_POS("", current_position);
} }
#endif #endif
const float nx = lx - (X_PROBE_OFFSET_FROM_EXTRUDER), ny = ly - (Y_PROBE_OFFSET_FROM_EXTRUDER); const float nx = rx - (X_PROBE_OFFSET_FROM_EXTRUDER), ny = ry - (Y_PROBE_OFFSET_FROM_EXTRUDER);
if (printable if (printable
? !position_is_reachable_xy(nx, ny) ? !position_is_reachable(nx, ny)
: !position_is_reachable_by_probe_xy(lx, ly) : !position_is_reachable_by_probe(rx, ry)
) return NAN; ) return NAN;
@ -634,9 +634,9 @@ float probe_pt(const float &lx, const float &ly, const bool stow, const uint8_t
if (verbose_level > 2) { if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Bed X: "); SERIAL_PROTOCOLPGM("Bed X: ");
SERIAL_PROTOCOL_F(lx, 3); SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(rx), 3);
SERIAL_PROTOCOLPGM(" Y: "); SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL_F(ly, 3); SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(ry), 3);
SERIAL_PROTOCOLPGM(" Z: "); SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL_F(measured_z, 3); SERIAL_PROTOCOL_F(measured_z, 3);
SERIAL_EOL(); SERIAL_EOL();

2
Marlin/src/module/probe.h
View file

@ -30,7 +30,7 @@
#include "../inc/MarlinConfig.h" #include "../inc/MarlinConfig.h"
bool set_probe_deployed(const bool deploy); bool set_probe_deployed(const bool deploy);
float probe_pt(const float &lx, const float &ly, const bool, const uint8_t, const bool printable=true); float probe_pt(const float &rx, const float &ry, const bool, const uint8_t, const bool printable=true);
#if HAS_BED_PROBE #if HAS_BED_PROBE
extern float zprobe_zoffset; extern float zprobe_zoffset;

16
Marlin/src/module/scara.cpp
View file

@ -36,14 +36,14 @@ float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
void scara_set_axis_is_at_home(const AxisEnum axis) { void scara_set_axis_is_at_home(const AxisEnum axis) {
if (axis == Z_AXIS) if (axis == Z_AXIS)
current_position[Z_AXIS] = LOGICAL_POSITION(Z_HOME_POS, Z_AXIS); current_position[Z_AXIS] = Z_HOME_POS;
else { else {
/** /**
* SCARA homes XY at the same time * SCARA homes XY at the same time
*/ */
float homeposition[XYZ]; float homeposition[XYZ];
LOOP_XYZ(i) homeposition[i] = LOGICAL_POSITION(base_home_pos((AxisEnum)i), i); LOOP_XYZ(i) homeposition[i] = base_home_pos((AxisEnum)i);
// SERIAL_ECHOPAIR("homeposition X:", homeposition[X_AXIS]); // SERIAL_ECHOPAIR("homeposition X:", homeposition[X_AXIS]);
// SERIAL_ECHOLNPAIR(" Y:", homeposition[Y_AXIS]); // SERIAL_ECHOLNPAIR(" Y:", homeposition[Y_AXIS]);
@ -58,7 +58,7 @@ void scara_set_axis_is_at_home(const AxisEnum axis) {
// SERIAL_ECHOPAIR("Cartesian X:", cartes[X_AXIS]); // SERIAL_ECHOPAIR("Cartesian X:", cartes[X_AXIS]);
// SERIAL_ECHOLNPAIR(" Y:", cartes[Y_AXIS]); // SERIAL_ECHOLNPAIR(" Y:", cartes[Y_AXIS]);
current_position[axis] = LOGICAL_POSITION(cartes[axis], axis); current_position[axis] = cartes[axis];
/** /**
* SCARA home positions are based on configuration since the actual * SCARA home positions are based on configuration since the actual
@ -104,12 +104,12 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
* Maths and first version by QHARLEY. * Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny. * Integrated into Marlin and slightly restructured by Joachim Cerny.
*/ */
void inverse_kinematics(const float logical[XYZ]) { void inverse_kinematics(const float raw[XYZ]) {
static float C2, S2, SK1, SK2, THETA, PSI; static float C2, S2, SK1, SK2, THETA, PSI;
float sx = RAW_X_POSITION(logical[X_AXIS]) - SCARA_OFFSET_X, // Translate SCARA to standard X Y float sx = raw[X_AXIS] - SCARA_OFFSET_X, // Translate SCARA to standard X Y
sy = RAW_Y_POSITION(logical[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor. sy = raw[Y_AXIS] - SCARA_OFFSET_Y; // With scaling factor.
if (L1 == L2) if (L1 == L2)
C2 = HYPOT2(sx, sy) / L1_2_2 - 1; C2 = HYPOT2(sx, sy) / L1_2_2 - 1;
@ -132,10 +132,10 @@ void inverse_kinematics(const float logical[XYZ]) {
delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle
delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor)
delta[C_AXIS] = logical[Z_AXIS]; delta[C_AXIS] = raw[Z_AXIS];
/* /*
DEBUG_POS("SCARA IK", logical); DEBUG_POS("SCARA IK", raw);
DEBUG_POS("SCARA IK", delta); DEBUG_POS("SCARA IK", delta);
SERIAL_ECHOPAIR(" SCARA (x,y) ", sx); SERIAL_ECHOPAIR(" SCARA (x,y) ", sx);
SERIAL_ECHOPAIR(",", sy); SERIAL_ECHOPAIR(",", sy);

2
Marlin/src/module/scara.h
View file

@ -38,7 +38,7 @@ float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
void scara_set_axis_is_at_home(const AxisEnum axis); void scara_set_axis_is_at_home(const AxisEnum axis);
void inverse_kinematics(const float logical[XYZ]); void inverse_kinematics(const float raw[XYZ]);
void forward_kinematics_SCARA(const float &a, const float &b); void forward_kinematics_SCARA(const float &a, const float &b);
void scara_report_positions(); void scara_report_positions();

8
Marlin/src/module/tool_change.cpp
View file

@ -240,9 +240,9 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
switch (dual_x_carriage_mode) { switch (dual_x_carriage_mode) {
case DXC_FULL_CONTROL_MODE: case DXC_FULL_CONTROL_MODE:
// New current position is the position of the activated extruder // New current position is the position of the activated extruder
current_position[X_AXIS] = LOGICAL_X_POSITION(inactive_extruder_x_pos); current_position[X_AXIS] = inactive_extruder_x_pos;
// Save the inactive extruder's position (from the old current_position) // Save the inactive extruder's position (from the old current_position)
inactive_extruder_x_pos = RAW_X_POSITION(destination[X_AXIS]); inactive_extruder_x_pos = destination[X_AXIS];
break; break;
case DXC_AUTO_PARK_MODE: case DXC_AUTO_PARK_MODE:
// record raised toolhead position for use by unpark // record raised toolhead position for use by unpark
@ -260,10 +260,10 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
active_extruder_parked = (active_extruder == 0); active_extruder_parked = (active_extruder == 0);
if (active_extruder_parked) if (active_extruder_parked)
current_position[X_AXIS] = LOGICAL_X_POSITION(inactive_extruder_x_pos); current_position[X_AXIS] = inactive_extruder_x_pos;
else else
current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset; current_position[X_AXIS] = destination[X_AXIS] + duplicate_extruder_x_offset;
inactive_extruder_x_pos = RAW_X_POSITION(destination[X_AXIS]); inactive_extruder_x_pos = destination[X_AXIS];
extruder_duplication_enabled = false; extruder_duplication_enabled = false;
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {