Merge pull request #8730 from thinkyhead/bf2_fixup_ubl

[2.0.x] UBL - Skew and Dual X Carriage
This commit is contained in:
Scott Lahteine 2017-12-11 03:37:13 -06:00 committed by GitHub
commit c555a214d2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
16 changed files with 435 additions and 404 deletions

View file

@ -79,13 +79,14 @@ script:
- opt_set TEMP_SENSOR_3 20 - opt_set TEMP_SENSOR_3 20
- opt_set TEMP_SENSOR_4 999 - opt_set TEMP_SENSOR_4 999
- opt_set TEMP_SENSOR_BED 1 - opt_set TEMP_SENSOR_BED 1
- opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL - opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL SKEW_CORRECTION
- opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING LIN_ADVANCE NANODLP_Z_SYNC - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING LIN_ADVANCE NANODLP_Z_SYNC
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}
# #
# And with a Sled Z Probe # Add a Sled Z Probe, do non-segmented moves
# #
- opt_enable Z_PROBE_SLED - opt_enable Z_PROBE_SLED
- opt_disable SEGMENT_LEVELED_MOVES
- opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING - opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}
# #
@ -121,7 +122,7 @@ script:
- opt_enable ULTIMAKERCONTROLLER SDSUPPORT - opt_enable ULTIMAKERCONTROLLER SDSUPPORT
- opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG - opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG
- opt_enable_adv BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS - opt_enable_adv BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS
- opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU - opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU M114_DETAIL
- opt_set_adv PWM_MOTOR_CURRENT {1300,1300,1250} - opt_set_adv PWM_MOTOR_CURRENT {1300,1300,1250}
- opt_set_adv I2C_SLAVE_ADDRESS 63 - opt_set_adv I2C_SLAVE_ADDRESS 63
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}

View file

@ -55,6 +55,59 @@
safe_delay(10); safe_delay(10);
} }
#if ENABLED(UBL_DEVEL_DEBUGGING)
static void debug_echo_axis(const AxisEnum axis) {
if (current_position[axis] == destination[axis])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
}
void debug_current_and_destination(const char *title) {
// if the title message starts with a '!' it is so important, we are going to
// ignore the status of the g26_debug_flag
if (*title != '!' && !g26_debug_flag) return;
const float de = destination[E_AXIS] - current_position[E_AXIS];
if (de == 0.0) return; // Printing moves only
const float dx = destination[X_AXIS] - current_position[X_AXIS],
dy = destination[Y_AXIS] - current_position[Y_AXIS],
xy_dist = HYPOT(dx, dy);
if (xy_dist == 0.0) return;
SERIAL_ECHOPGM(" fpmm=");
const float fpmm = de / xy_dist;
SERIAL_ECHO_F(fpmm, 6);
SERIAL_ECHOPGM(" current=( ");
SERIAL_ECHO_F(current_position[X_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Y_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Z_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( ");
debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title);
SERIAL_EOL();
}
#endif // UBL_DEVEL_DEBUGGING
int8_t unified_bed_leveling::storage_slot; int8_t unified_bed_leveling::storage_slot;
float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
@ -178,7 +231,7 @@
uint8_t error_flag = 0; uint8_t error_flag = 0;
if (settings.calc_num_meshes() < 1) { if (settings.calc_num_meshes() < 1) {
SERIAL_PROTOCOLLNPGM("?Insufficient EEPROM storage for a mesh of this size."); SERIAL_PROTOCOLLNPGM("?Mesh too big for EEPROM.");
error_flag++; error_flag++;
} }

View file

@ -23,6 +23,8 @@
#ifndef UNIFIED_BED_LEVELING_H #ifndef UNIFIED_BED_LEVELING_H
#define UNIFIED_BED_LEVELING_H #define UNIFIED_BED_LEVELING_H
//#define UBL_DEVEL_DEBUGGING
#include "../bedlevel.h" #include "../bedlevel.h"
#include "../../../module/planner.h" #include "../../../module/planner.h"
#include "../../../module/motion.h" #include "../../../module/motion.h"
@ -37,7 +39,11 @@
// ubl_motion.cpp // ubl_motion.cpp
void debug_current_and_destination(const char * const title); #if ENABLED(UBL_DEVEL_DEBUGGING)
void debug_current_and_destination(const char * const title);
#else
FORCE_INLINE void debug_current_and_destination(const char * const title) { UNUSED(title); }
#endif
// ubl_G29.cpp // ubl_G29.cpp
@ -315,8 +321,11 @@ 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 rtarget[XYZE], const float &feedrate); #if UBL_SEGMENTED
static void line_to_destination_cartesian(const float &fr, uint8_t e); static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
#else
static void line_to_destination_cartesian(const float &fr, const uint8_t e);
#endif
#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])
#define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1)) #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))

View file

@ -23,93 +23,42 @@
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
#include "../bedlevel.h" #include "../bedlevel.h"
#include "../../../module/planner.h" #include "../../../module/planner.h"
#include "../../../module/stepper.h" #include "../../../module/stepper.h"
#include "../../../module/motion.h" #include "../../../module/motion.h"
#if ENABLED(DELTA) #if ENABLED(DELTA)
#include "../../../module/delta.h" #include "../../../module/delta.h"
#endif #endif
#include "../../../Marlin.h" #include "../../../Marlin.h"
#include <math.h> #include <math.h>
extern float destination[XYZE]; #if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
inline void set_current_from_destination() { COPY(current_position, destination); } inline void set_current_from_destination() { COPY(current_position, destination); }
#else #else
extern void set_current_from_destination(); extern void set_current_from_destination();
#endif #endif
static void debug_echo_axis(const AxisEnum axis) { #if !UBL_SEGMENTED
if (current_position[axis] == destination[axis])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
}
void debug_current_and_destination(const char *title) { void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
// if the title message starts with a '!' it is so important, we are going to
// ignore the status of the g26_debug_flag
if (*title != '!' && !g26_debug_flag) return;
const float de = destination[E_AXIS] - current_position[E_AXIS];
if (de == 0.0) return; // Printing moves only
const float dx = destination[X_AXIS] - current_position[X_AXIS],
dy = destination[Y_AXIS] - current_position[Y_AXIS],
xy_dist = HYPOT(dx, dy);
if (xy_dist == 0.0) return;
SERIAL_ECHOPGM(" fpmm=");
const float fpmm = de / xy_dist;
SERIAL_ECHO_F(fpmm, 6);
SERIAL_ECHOPGM(" current=( ");
SERIAL_ECHO_F(current_position[X_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Y_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Z_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( ");
debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title);
SERIAL_EOL();
}
void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
/** /**
* Much of the nozzle movement will be within the same cell. So we will do as little computation * Much of the nozzle movement will be within the same cell. So we will do as little computation
* as possible to determine if this is the case. If this move is within the same cell, we will * as possible to determine if this is the case. If this move is within the same cell, we will
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/ */
const float start[XYZE] = { #if ENABLED(SKEW_CORRECTION)
current_position[X_AXIS], // For skew correction just adjust the destination point and we're done
current_position[Y_AXIS], float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
current_position[Z_AXIS], end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
current_position[E_AXIS] planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
}, planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
end[XYZE] = { #else
destination[X_AXIS], const float (&start)[XYZE] = current_position,
destination[Y_AXIS], (&end)[XYZE] = destination;
destination[Z_AXIS], #endif
destination[E_AXIS]
};
const int cell_start_xi = get_cell_index_x(start[X_AXIS]), const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]), cell_start_yi = get_cell_index_y(start[Y_AXIS]),
@ -117,13 +66,13 @@
cell_dest_yi = get_cell_index_y(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_cartesian(xe=", destination[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL(); SERIAL_EOL();
debug_current_and_destination(PSTR("Start of ubl.line_to_destination()")); debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
} }
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell, if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
@ -139,11 +88,11 @@
// Note: There is no Z Correction in this case. We are off the grid and don't know what // Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be. // a reasonable correction would be.
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder); planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
set_current_from_destination(); set_current_from_destination();
if (g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()")); debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
return; return;
} }
@ -183,10 +132,10 @@
*/ */
if (isnan(z0)) z0 = 0.0; if (isnan(z0)) z0 = 0.0;
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder); planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
if (g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
set_current_from_destination(); set_current_from_destination();
return; return;
@ -274,7 +223,7 @@
* 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
* where the line is heading down and it is starting right on a Mesh Line boundary. For how often that * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
* 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_segment() routine will filter it if that happens.
*/ */
if (ry != start[Y_AXIS]) { if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
@ -287,12 +236,12 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
if (g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()")); debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
// //
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done. // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
@ -338,7 +287,7 @@
* 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
* where the line is heading left and it is starting right on a Mesh Line boundary. For how often * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
* 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_segment() routine will filter it if that happens.
*/ */
if (rx != start[X_AXIS]) { if (rx != start[X_AXIS]) {
if (!inf_normalized_flag) { if (!inf_normalized_flag) {
@ -351,12 +300,12 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED "); } //else printf("FIRST MOVE PRUNED ");
} }
if (g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()")); debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE; goto FINAL_MOVE;
@ -413,7 +362,7 @@
e_position = end[E_AXIS]; e_position = end[E_AXIS];
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
current_yi += dyi; current_yi += dyi;
yi_cnt--; yi_cnt--;
} }
@ -441,7 +390,7 @@
z_position = end[Z_AXIS]; z_position = end[Z_AXIS];
} }
planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
current_xi += dxi; current_xi += dxi;
xi_cnt--; xi_cnt--;
} }
@ -450,7 +399,7 @@
} }
if (g26_debug_flag) if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()")); debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE; goto FINAL_MOVE;
@ -458,29 +407,28 @@
set_current_from_destination(); set_current_from_destination();
} }
#if UBL_DELTA #else // UBL_SEGMENTED
// macro to inline copy exactly 4 floats, don't rely on sizeof operator
#define COPY_XYZE( target, source ) { \
target[X_AXIS] = source[X_AXIS]; \
target[Y_AXIS] = source[Y_AXIS]; \
target[Z_AXIS] = source[Z_AXIS]; \
target[E_AXIS] = source[E_AXIS]; \
}
#if IS_SCARA // scale the feed rate from mm/s to degrees/s #if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor, scara_oldA, scara_oldB; static float scara_feed_factor, scara_oldA, scara_oldB;
#endif #endif
// 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_segment directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) { inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
#endif
#if ENABLED(DELTA) // apply delta inverse_kinematics #if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_RAW_IK(); DELTA_RAW_IK();
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], 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)
@ -493,11 +441,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], raw[E_AXIS], s_feedrate, active_extruder); planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN #else // CARTESIAN
planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder); planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif #endif
} }
@ -516,11 +464,11 @@
/** /**
* Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
* This calls planner._buffer_line multiple times for small incremental moves. * This calls planner.buffer_segment multiple times for small incremental moves.
* 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 rtarget[XYZE], const float &feedrate) { bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[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
@ -675,6 +623,6 @@
} // cell loop } // cell loop
} }
#endif // UBL_DELTA #endif // UBL_SEGMENTED
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL

View file

@ -220,7 +220,7 @@ mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) {
void G26_line_to_destination(const float &feed_rate) { void G26_line_to_destination(const float &feed_rate) {
const float save_feedrate = feedrate_mm_s; const float save_feedrate = feedrate_mm_s;
feedrate_mm_s = feed_rate; // use specified feed rate feedrate_mm_s = feed_rate; // use specified feed rate
prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_DELTA prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_SEGMENTED
feedrate_mm_s = save_feedrate; // restore global feed rate feedrate_mm_s = save_feedrate; // restore global feed rate
} }
@ -261,16 +261,16 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de
set_destination_from_current(); set_destination_from_current();
} }
FORCE_INLINE void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); } FORCE_INLINE void move_to(const float (&where)[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
void retract_filament(const float where[XYZE]) { void retract_filament(const float (&where)[XYZE]) {
if (!g26_retracted) { // Only retract if we are not already retracted! if (!g26_retracted) { // Only retract if we are not already retracted!
g26_retracted = true; g26_retracted = true;
move_to(where, -1.0 * g26_retraction_multiplier); move_to(where, -1.0 * g26_retraction_multiplier);
} }
} }
void recover_filament(const float where[XYZE]) { void recover_filament(const float (&where)[XYZE]) {
if (g26_retracted) { // Only un-retract if we are retracted. if (g26_retracted) { // Only un-retract if we are retracted.
move_to(where, 1.2 * g26_retraction_multiplier); move_to(where, 1.2 * g26_retraction_multiplier);
g26_retracted = false; g26_retracted = false;

View file

@ -28,7 +28,7 @@
#if ENABLED(M114_DETAIL) #if ENABLED(M114_DETAIL)
void report_xyze(const float pos[XYZE], const uint8_t n = 4, const uint8_t precision = 3) { void report_xyze(const float pos[], const uint8_t n = 4, const uint8_t precision = 3) {
char str[12]; char str[12];
for (uint8_t i = 0; i < n; i++) { for (uint8_t i = 0; i < n; i++) {
SERIAL_CHAR(' '); SERIAL_CHAR(' ');
@ -39,7 +39,7 @@
SERIAL_EOL(); SERIAL_EOL();
} }
inline void report_xyz(const float pos[XYZ]) { report_xyze(pos, 3); } inline void report_xyz(const float pos[]) { report_xyze(pos, 3); }
void report_current_position_detail() { void report_current_position_detail() {
@ -80,8 +80,13 @@
#endif #endif
SERIAL_PROTOCOLPGM("Stepper:"); SERIAL_PROTOCOLPGM("Stepper:");
const float step_count[XYZE] = { stepper.position(X_AXIS), stepper.position(Y_AXIS), stepper.position(Z_AXIS), stepper.position(E_AXIS) }; LOOP_XYZE(i) {
report_xyze(step_count, 4, 0); SERIAL_CHAR(' ');
SERIAL_CHAR(axis_codes[i]);
SERIAL_CHAR(':');
SERIAL_PROTOCOL(stepper.position((AxisEnum)i));
}
SERIAL_EOL();
#if IS_SCARA #if IS_SCARA
const float deg[XYZ] = { const float deg[XYZ] = {

View file

@ -44,9 +44,9 @@
* 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 rtarget[XYZE], // Destination position const float (&cart)[XYZE], // Destination position
float *offset, // Center of rotation relative to current_position const float (&offset)[2], // Center of rotation relative to current_position
uint8_t clockwise // Clockwise? const uint8_t clockwise // Clockwise?
) { ) {
#if ENABLED(CNC_WORKSPACE_PLANES) #if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis; AxisEnum p_axis, q_axis, l_axis;
@ -66,10 +66,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 = rtarget[p_axis] - center_P, rt_X = cart[p_axis] - center_P,
rt_Y = rtarget[q_axis] - center_Q, rt_Y = cart[q_axis] - center_Q,
linear_travel = rtarget[l_axis] - current_position[l_axis], linear_travel = cart[l_axis] - current_position[l_axis],
extruder_travel = rtarget[E_AXIS] - current_position[E_AXIS]; extruder_travel = cart[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);
@ -77,7 +77,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] == rtarget[p_axis] && current_position[q_axis] == rtarget[q_axis]) if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[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));
@ -177,7 +177,7 @@ void plan_arc(
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
planner.buffer_line_kinematic(rtarget, fr_mm_s, active_extruder); planner.buffer_line_kinematic(cart, 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

View file

@ -27,7 +27,7 @@
#include "../../module/motion.h" #include "../../module/motion.h"
#include "../../module/planner_bezier.h" #include "../../module/planner_bezier.h"
void plan_cubic_move(const float offset[4]) { void plan_cubic_move(const float (&offset)[4]) {
cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder); cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
// As far as the parser is concerned, the position is now == destination. In reality the // As far as the parser is concerned, the position is now == destination. In reality the
@ -62,7 +62,7 @@ void GcodeSuite::G5() {
get_destination_from_command(); get_destination_from_command();
const float offset[] = { const float offset[4] = {
parser.linearval('I'), parser.linearval('I'),
parser.linearval('J'), parser.linearval('J'),
parser.linearval('P'), parser.linearval('P'),

View file

@ -977,7 +977,7 @@
/** /**
* Set granular options based on the specific type of leveling * Set granular options based on the specific type of leveling
*/ */
#define UBL_DELTA (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES))) #define UBL_SEGMENTED (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES)))
#define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT)) #define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
#define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR)) #define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
#define OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID) #define OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID)
@ -985,7 +985,7 @@
#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
#define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY)) #define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY))
#define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING)) #define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA) #define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#if HAS_PROBING_PROCEDURE #if HAS_PROBING_PROCEDURE
#define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION)) #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))

View file

@ -603,7 +603,7 @@ static_assert(1 >= 0
#error "Delta probably shouldn't use Z_MIN_PROBE_ENDSTOP. Comment out this line to continue." #error "Delta probably shouldn't use Z_MIN_PROBE_ENDSTOP. Comment out this line to continue."
#elif DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG) #elif DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
#error "You probably want to use Max Endstops for DELTA!" #error "You probably want to use Max Endstops for DELTA!"
#elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_SEGMENTED
#error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL." #error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL."
#elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL)) #elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL))
#error "DELTA_AUTO_CALIBRATION requires a probe or LCD Controller." #error "DELTA_AUTO_CALIBRATION requires a probe or LCD Controller."
@ -1497,9 +1497,6 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m
#endif #endif
#if ENABLED(SKEW_CORRECTION) #if ENABLED(SKEW_CORRECTION)
#if ENABLED(AUTO_BED_LEVELING_UBL) && !ENABLED(SEGMENT_LEVELED_MOVES)
#error "SKEW_CORRECTION with AUTO_BED_LEVELING_UBL requires SEGMENT_LEVELED_MOVES."
#endif
#if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD)) #if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD))
#error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD." #error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD."
#endif #endif

View file

@ -264,7 +264,7 @@ void buffer_line_to_destination(const float fr_mm_s) {
gcode.refresh_cmd_timeout(); gcode.refresh_cmd_timeout();
#if UBL_DELTA #if UBL_SEGMENTED
// ubl segmented line will do z-only moves in single segment // ubl segmented line will do z-only moves in single segment
ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s)); ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
#else #else
@ -495,7 +495,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#endif #endif
#if !UBL_DELTA #if !UBL_SEGMENTED
#if IS_KINEMATIC #if IS_KINEMATIC
#if ENABLED(AUTO_BED_LEVELING_BILINEAR) #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
@ -517,13 +517,19 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
/** /**
* Prepare a linear move in a DELTA or SCARA setup. * Prepare a linear move in a DELTA or SCARA setup.
* *
* Called from prepare_move_to_destination as the
* default Delta/SCARA segmenter.
*
* 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.
* *
* For Unified Bed Leveling (Delta or Segmented Cartesian) * For Unified Bed Leveling (Delta or Segmented Cartesian)
* the ubl.prepare_segmented_line_to method replaces this. * the ubl.prepare_segmented_line_to method replaces this.
*
* For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES
* this is replaced by segmented_line_to_destination below.
*/ */
inline bool prepare_kinematic_move_to(float rtarget[XYZE]) { inline bool prepare_kinematic_move_to(const 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);
@ -756,7 +762,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
} }
#endif // !IS_KINEMATIC #endif // !IS_KINEMATIC
#endif // !UBL_DELTA #endif // !UBL_SEGMENTED
#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE) #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
bool extruder_duplication_enabled = false; // Used in Dual X mode 2 bool extruder_duplication_enabled = false; // Used in Dual X mode 2
@ -790,7 +796,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
* *
* Return true if current_position[] was set to destination[] * Return true if current_position[] was set to destination[]
*/ */
inline bool prepare_move_to_destination_dualx() { inline bool dual_x_carriage_unpark() {
if (active_extruder_parked) { if (active_extruder_parked) {
switch (dual_x_carriage_mode) { switch (dual_x_carriage_mode) {
case DXC_FULL_CONTROL_MODE: case DXC_FULL_CONTROL_MODE:
@ -859,7 +865,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
break; break;
} }
} }
return prepare_move_to_destination_cartesian(); return false;
} }
#endif // DUAL_X_CARRIAGE #endif // DUAL_X_CARRIAGE
@ -900,13 +906,15 @@ void prepare_move_to_destination() {
#endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
#if ENABLED(DUAL_X_CARRIAGE)
if (dual_x_carriage_unpark()) return;
#endif
if ( if (
#if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely) #if UBL_SEGMENTED
ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s)) ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
#elif IS_KINEMATIC #elif IS_KINEMATIC
prepare_kinematic_move_to(destination) prepare_kinematic_move_to(destination)
#elif ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx()
#else #else
prepare_move_to_destination_cartesian() prepare_move_to_destination_cartesian()
#endif #endif

View file

@ -580,14 +580,7 @@ void Planner::calculate_volumetric_multipliers() {
void Planner::apply_leveling(float &rx, float &ry, float &rz) { void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION) #if ENABLED(SKEW_CORRECTION)
if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) { skew(rx, ry, rz);
const float tempry = ry - (rz * planner.yz_skew_factor),
temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor)));
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
rx = temprx;
ry = tempry;
}
}
#endif #endif
if (!leveling_active) return; if (!leveling_active) return;
@ -616,7 +609,7 @@ void Planner::calculate_volumetric_multipliers() {
#endif #endif
rz += ( rz += (
#if ENABLED(AUTO_BED_LEVELING_UBL) // UBL_DELTA #if ENABLED(AUTO_BED_LEVELING_UBL)
ubl.get_z_correction(rx, ry) * fade_scaling_factor ubl.get_z_correction(rx, ry) * fade_scaling_factor
#elif ENABLED(MESH_BED_LEVELING) #elif ENABLED(MESH_BED_LEVELING)
mbl.get_z(rx, ry mbl.get_z(rx, ry
@ -678,14 +671,7 @@ void Planner::calculate_volumetric_multipliers() {
} }
#if ENABLED(SKEW_CORRECTION) #if ENABLED(SKEW_CORRECTION)
if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor,
tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor;
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
raw[X_AXIS] = temprx;
raw[Y_AXIS] = tempry;
}
}
#endif #endif
} }
@ -1365,7 +1351,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
} // _buffer_steps() } // _buffer_steps()
/** /**
* Planner::_buffer_line * Planner::buffer_segment
* *
* Add a new linear movement to the buffer in axis units. * Add a new linear movement to the buffer in axis units.
* *
@ -1375,7 +1361,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - target extruder
*/ */
void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) { void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
// When changing extruders recalculate steps corresponding to the E position // When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS) #if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) { if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@ -1394,7 +1380,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
}; };
/* <-- add a slash to enable /* <-- add a slash to enable
SERIAL_ECHOPAIR(" _buffer_line FR:", fr_mm_s); SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s);
#if IS_KINEMATIC #if IS_KINEMATIC
SERIAL_ECHOPAIR(" A:", a); SERIAL_ECHOPAIR(" A:", a);
SERIAL_ECHOPAIR(" (", position[A_AXIS]); SERIAL_ECHOPAIR(" (", position[A_AXIS]);
@ -1441,7 +1427,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
stepper.wake_up(); stepper.wake_up();
} // _buffer_line() } // buffer_segment()
/** /**
* Directly set the planner XYZ position (and stepper positions) * Directly set the planner XYZ position (and stepper positions)
@ -1466,18 +1452,18 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
ZERO(previous_speed); ZERO(previous_speed);
} }
void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) { void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#if PLANNER_LEVELING #if PLANNER_LEVELING
float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] }; float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(lpos); apply_leveling(raw);
#else #else
const float * const lpos = position; const float (&raw)[XYZE] = cart;
#endif #endif
#if IS_KINEMATIC #if IS_KINEMATIC
inverse_kinematics(lpos); inverse_kinematics(raw);
_set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]); _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
#else #else
_set_position_mm(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], position[E_AXIS]); _set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
#endif #endif
} }

View file

@ -146,7 +146,7 @@ class Planner {
* head!=tail : blocks are in the buffer * head!=tail : blocks are in the buffer
* head==(tail-1)%size : the buffer is full * head==(tail-1)%size : the buffer is full
* *
* Writer of head is Planner::_buffer_line(). * Writer of head is Planner::buffer_segment().
* Reader of tail is Stepper::isr(). Always consider tail busy / read-only * Reader of tail is Stepper::isr(). Always consider tail busy / read-only
*/ */
static block_t block_buffer[BLOCK_BUFFER_SIZE]; static block_t block_buffer[BLOCK_BUFFER_SIZE];
@ -345,6 +345,30 @@ class Planner {
#endif #endif
#if ENABLED(SKEW_CORRECTION)
FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
const float sx = cx - (cy * xy_skew_factor) - (cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor))),
sy = cy - (cz * yz_skew_factor);
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,
sy = cy + cz * yz_skew_factor;
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
#endif // SKEW_CORRECTION
#if PLANNER_LEVELING #if PLANNER_LEVELING
#define ARG_X float rx #define ARG_X float rx
@ -356,7 +380,7 @@ class Planner {
* 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 &rx, float &ry, float &rz); static void apply_leveling(float &rx, float &ry, float &rz);
static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[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 raw[XYZ]); static void unapply_leveling(float raw[XYZ]);
#else #else
@ -379,7 +403,7 @@ class Planner {
static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder); static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder);
/** /**
* Planner::_buffer_line * Planner::buffer_segment
* *
* Add a new linear movement to the buffer in axis units. * Add a new linear movement to the buffer in axis units.
* *
@ -389,7 +413,7 @@ class Planner {
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - target extruder
*/ */
static void _buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder); static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e); static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@ -409,7 +433,7 @@ class Planner {
#if PLANNER_LEVELING && IS_CARTESIAN #if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz); apply_leveling(rx, ry, rz);
#endif #endif
_buffer_line(rx, ry, rz, e, fr_mm_s, extruder); buffer_segment(rx, ry, rz, e, fr_mm_s, extruder);
} }
/** /**
@ -421,18 +445,18 @@ class Planner {
* 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
*/ */
FORCE_INLINE static void buffer_line_kinematic(const float cart[XYZE], const float &fr_mm_s, const uint8_t extruder) { FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING #if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw); apply_leveling(raw);
#else #else
const float * const raw = cart; const float (&raw)[XYZE] = cart;
#endif #endif
#if IS_KINEMATIC #if IS_KINEMATIC
inverse_kinematics(raw); inverse_kinematics(raw);
_buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder); buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
#else #else
_buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder); buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
#endif #endif
} }
@ -451,7 +475,7 @@ class Planner {
#endif #endif
_set_position_mm(rx, ry, rz, 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 (&cart)[XYZE]);
static void set_position_mm(const AxisEnum axis, const float &v); static void set_position_mm(const AxisEnum axis, const float &v);
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); } FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }

View file

@ -107,7 +107,7 @@ 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 raw[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(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s); do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
} }

View file

@ -1193,7 +1193,7 @@ void Stepper::set_e_position(const long &e) {
/** /**
* Get a stepper's position in steps. * Get a stepper's position in steps.
*/ */
long Stepper::position(AxisEnum axis) { long Stepper::position(const AxisEnum axis) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
const long count_pos = count_position[axis]; const long count_pos = count_position[axis];
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
@ -1204,7 +1204,7 @@ long Stepper::position(AxisEnum axis) {
* Get an axis position according to stepper position(s) * Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ. * For CORE machines apply translation from ABC to XYZ.
*/ */
float Stepper::get_axis_position_mm(AxisEnum axis) { float Stepper::get_axis_position_mm(const AxisEnum axis) {
float axis_steps; float axis_steps;
#if IS_CORE #if IS_CORE
// Requesting one of the "core" axes? // Requesting one of the "core" axes?
@ -1242,7 +1242,7 @@ void Stepper::quick_stop() {
#endif #endif
} }
void Stepper::endstop_triggered(AxisEnum axis) { void Stepper::endstop_triggered(const AxisEnum axis) {
#if IS_CORE #if IS_CORE

View file

@ -183,7 +183,7 @@ class Stepper {
// //
// Get the position of a stepper, in steps // Get the position of a stepper, in steps
// //
static long position(AxisEnum axis); static long position(const AxisEnum axis);
// //
// Report the positions of the steppers, in steps // Report the positions of the steppers, in steps
@ -193,13 +193,13 @@ class Stepper {
// //
// Get the position (mm) of an axis based on stepper position(s) // Get the position (mm) of an axis based on stepper position(s)
// //
static float get_axis_position_mm(AxisEnum axis); static float get_axis_position_mm(const AxisEnum axis);
// //
// SCARA AB axes are in degrees, not mm // SCARA AB axes are in degrees, not mm
// //
#if IS_SCARA #if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); } FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif #endif
// //
@ -221,7 +221,7 @@ class Stepper {
// //
// The direction of a single motor // The direction of a single motor
// //
FORCE_INLINE static bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); } FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
#if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digitalPotWrite(const int16_t address, const int16_t value); static void digitalPotWrite(const int16_t address, const int16_t value);
@ -263,12 +263,12 @@ class Stepper {
// //
// Handle a triggered endstop // Handle a triggered endstop
// //
static void endstop_triggered(AxisEnum axis); static void endstop_triggered(const AxisEnum axis);
// //
// Triggered position of an axis in mm (not core-savvy) // Triggered position of an axis in mm (not core-savvy)
// //
FORCE_INLINE static float triggered_position_mm(AxisEnum axis) { FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
return endstops_trigsteps[axis] * planner.steps_to_mm[axis]; return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
} }