Merge pull request #8729 from thinkyhead/bf1_sort_out_leveling

[1.1.x] UBL - Skew and Dual X Carriage
master
Scott Lahteine 7 years ago committed by GitHub
commit 17b05c150c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

@ -103,9 +103,10 @@ script:
- opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING NANODLP_Z_SYNC - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING NANODLP_Z_SYNC
- build_marlin - build_marlin
# #
# And with a Sled Z Probe # Add a Sled Z Probe, use UBL Cartesian moves
# #
- opt_enable Z_PROBE_SLED - opt_enable Z_PROBE_SLED SKEW_CORRECTION SKEW_CORRECTION_FOR_Z SKEW_CORRECTION_GCODE
- 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 - build_marlin
# #
@ -141,7 +142,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 - build_marlin

@ -933,7 +933,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)
@ -941,7 +941,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 || ENABLED(SKEW_CORRECTION)) #define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION))
#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))

@ -137,7 +137,6 @@
#if ENABLED(ULTRA_LCD) #if ENABLED(ULTRA_LCD)
extern char lcd_status_message[]; extern char lcd_status_message[];
#endif #endif
extern float destination[XYZE];
void set_destination_from_current(); void set_destination_from_current();
void prepare_move_to_destination(); void prepare_move_to_destination();
inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
@ -189,7 +188,7 @@
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
} }

@ -223,7 +223,7 @@ extern volatile bool wait_for_heatup;
extern volatile bool wait_for_user; extern volatile bool wait_for_user;
#endif #endif
extern float current_position[NUM_AXIS]; extern float current_position[XYZE], destination[XYZE];
// Workspace offsets // Workspace offsets
#if HAS_WORKSPACE_OFFSET #if HAS_WORKSPACE_OFFSET

@ -706,7 +706,7 @@ FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_by
#define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \ #define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
static const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \ static const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \
static inline type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \ static inline type array(const AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
typedef void __void_##CONFIG##__ typedef void __void_##CONFIG##__
XYZ_CONSTS_FROM_CONFIG(float, base_min_pos, MIN_POS); XYZ_CONSTS_FROM_CONFIG(float, base_min_pos, MIN_POS);
@ -733,11 +733,11 @@ void get_cartesian_from_steppers();
void set_current_from_steppers_for_axis(const AxisEnum axis); void set_current_from_steppers_for_axis(const AxisEnum axis);
#if ENABLED(ARC_SUPPORT) #if ENABLED(ARC_SUPPORT)
void plan_arc(float target[XYZE], float* offset, uint8_t clockwise); void plan_arc(const float (&cart)[XYZE], const float (&offset)[2], const bool clockwise);
#endif #endif
#if ENABLED(BEZIER_CURVE_SUPPORT) #if ENABLED(BEZIER_CURVE_SUPPORT)
void plan_cubic_move(const float offset[4]); void plan_cubic_move(const float (&offset)[4]);
#endif #endif
void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false); void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
@ -1550,7 +1550,7 @@ inline void set_destination_from_current() { COPY(destination, current_position)
refresh_cmd_timeout(); 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
@ -1808,7 +1808,7 @@ static void clean_up_after_endstop_or_probe_move() {
#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);
} }
@ -8326,7 +8326,7 @@ void report_current_position() {
#ifdef M114_DETAIL #ifdef 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(' ');
@ -8337,7 +8337,7 @@ void report_current_position() {
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() {
@ -12647,7 +12647,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#endif // AUTO_BED_LEVELING_BILINEAR #endif // AUTO_BED_LEVELING_BILINEAR
#endif // IS_CARTESIAN #endif // IS_CARTESIAN
#if !UBL_DELTA #if !UBL_SEGMENTED
#if IS_KINEMATIC #if IS_KINEMATIC
/** /**
@ -12659,7 +12659,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
* 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.
*/ */
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);
@ -12819,7 +12819,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
} }
#endif // !IS_KINEMATIC #endif // !IS_KINEMATIC
#endif // !UBL_DELTA #endif // !UBL_SEGMENTED
#if ENABLED(DUAL_X_CARRIAGE) #if ENABLED(DUAL_X_CARRIAGE)
@ -12895,7 +12895,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
break; break;
} }
} }
return prepare_move_to_destination_cartesian(); return (
#if UBL_SEGMENTED
ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
#else
prepare_move_to_destination_cartesian()
#endif
);
} }
#endif // DUAL_X_CARRIAGE #endif // DUAL_X_CARRIAGE
@ -12937,12 +12943,12 @@ void prepare_move_to_destination() {
#endif #endif
if ( if (
#if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely) #if ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx()
#elif 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
@ -12968,9 +12974,9 @@ void prepare_move_to_destination() {
* 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 raw[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 bool 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;
@ -12990,10 +12996,10 @@ void prepare_move_to_destination() {
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 = raw[p_axis] - center_P, rt_X = cart[p_axis] - center_P,
rt_Y = raw[q_axis] - center_Q, rt_Y = cart[q_axis] - center_Q,
linear_travel = raw[l_axis] - current_position[l_axis], linear_travel = cart[l_axis] - current_position[l_axis],
extruder_travel = raw[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);
@ -13001,7 +13007,7 @@ void prepare_move_to_destination() {
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] == raw[p_axis] && current_position[q_axis] == raw[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));
@ -13101,7 +13107,7 @@ void prepare_move_to_destination() {
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
planner.buffer_line_kinematic(raw, 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
@ -13113,7 +13119,7 @@ void prepare_move_to_destination() {
#if ENABLED(BEZIER_CURVE_SUPPORT) #if ENABLED(BEZIER_CURVE_SUPPORT)
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

@ -590,7 +590,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 either a probe or an LCD Controller." #error "DELTA_AUTO_CALIBRATION requires either a probe or an LCD Controller."
@ -1539,9 +1539,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

@ -569,14 +569,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;
@ -605,7 +598,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
@ -667,14 +660,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
} }
@ -1354,7 +1340,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.
* *
@ -1364,7 +1350,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]) {
@ -1383,7 +1369,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]);
@ -1430,7 +1416,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)
@ -1455,18 +1441,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
} }

@ -142,7 +142,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];
@ -341,6 +341,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
@ -352,7 +376,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
@ -375,7 +399,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.
* *
@ -385,7 +409,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);
@ -405,7 +429,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);
} }
/** /**
@ -417,18 +441,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
} }
@ -447,7 +471,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); }

@ -1198,7 +1198,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;
@ -1209,7 +1209,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?

@ -209,7 +209,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
@ -219,13 +219,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
// //
@ -247,7 +247,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);
@ -287,12 +287,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];
} }

@ -94,7 +94,7 @@
static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); } static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); }
static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); } static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); }
uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); } uint8_t dac_current_get_percent(const AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
void dac_current_set_percents(const uint8_t pct[XYZE]) { void dac_current_set_percents(const uint8_t pct[XYZE]) {
LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]]; LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
mcp4728_setDrvPct(dac_channel_pct); mcp4728_setDrvPct(dac_channel_pct);

@ -51,7 +51,7 @@ void dac_current_percent(uint8_t channel, float val);
void dac_current_raw(uint8_t channel, uint16_t val); void dac_current_raw(uint8_t channel, uint16_t val);
void dac_print_values(); void dac_print_values();
void dac_commit_eeprom(); void dac_commit_eeprom();
uint8_t dac_current_get_percent(AxisEnum axis); uint8_t dac_current_get_percent(const AxisEnum axis);
void dac_current_set_percents(const uint8_t pct[XYZE]); void dac_current_set_percents(const uint8_t pct[XYZE]);
#endif // STEPPER_DAC_H #endif // STEPPER_DAC_H

@ -51,6 +51,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];
@ -174,7 +227,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++;
} }

@ -26,6 +26,9 @@
#include "MarlinConfig.h" #include "MarlinConfig.h"
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
//#define UBL_DEVEL_DEBUGGING
#include "Marlin.h" #include "Marlin.h"
#include "planner.h" #include "planner.h"
#include "math.h" #include "math.h"
@ -41,7 +44,11 @@
// ubl_motion.cpp // ubl_motion.cpp
#if ENABLED(UBL_DEVEL_DEBUGGING)
void debug_current_and_destination(const char * const title); 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
@ -319,8 +326,11 @@
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))

@ -24,8 +24,6 @@
#if ENABLED(AUTO_BED_LEVELING_UBL) #if ENABLED(AUTO_BED_LEVELING_UBL)
//#define UBL_DEVEL_DEBUGGING
#include "ubl.h" #include "ubl.h"
#include "Marlin.h" #include "Marlin.h"
#include "hex_print_routines.h" #include "hex_print_routines.h"
@ -1165,12 +1163,12 @@
static uint8_t ubl_state_at_invocation = 0; static uint8_t ubl_state_at_invocation = 0;
#ifdef UBL_DEVEL_DEBUGGING #if ENABLED(UBL_DEVEL_DEBUGGING)
static uint8_t ubl_state_recursion_chk = 0; static uint8_t ubl_state_recursion_chk = 0;
#endif #endif
void unified_bed_leveling::save_ubl_active_state_and_disable() { void unified_bed_leveling::save_ubl_active_state_and_disable() {
#ifdef UBL_DEVEL_DEBUGGING #if ENABLED(UBL_DEVEL_DEBUGGING)
ubl_state_recursion_chk++; ubl_state_recursion_chk++;
if (ubl_state_recursion_chk != 1) { if (ubl_state_recursion_chk != 1) {
SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row."); SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
@ -1186,7 +1184,7 @@
} }
void unified_bed_leveling::restore_ubl_active_state_and_leave() { void unified_bed_leveling::restore_ubl_active_state_and_leave() {
#ifdef UBL_DEVEL_DEBUGGING #if ENABLED(UBL_DEVEL_DEBUGGING)
if (--ubl_state_recursion_chk) { if (--ubl_state_recursion_chk) {
SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times."); SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
#if ENABLED(NEWPANEL) #if ENABLED(NEWPANEL)
@ -1267,7 +1265,7 @@
SERIAL_EOL(); SERIAL_EOL();
safe_delay(50); safe_delay(50);
#ifdef UBL_DEVEL_DEBUGGING #if ENABLED(UBL_DEVEL_DEBUGGING)
SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation); SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
SERIAL_EOL(); SERIAL_EOL();
SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk); SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);

@ -30,81 +30,30 @@
#include <avr/io.h> #include <avr/io.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) {
// 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) { void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const 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]),
@ -112,13 +61,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,
@ -134,11 +83,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;
} }
@ -178,10 +127,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;
@ -269,7 +218,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) {
@ -282,12 +231,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.
@ -333,7 +282,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) {
@ -346,12 +295,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;
@ -408,7 +357,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--;
} }
@ -436,7 +385,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--;
} }
@ -445,7 +394,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;
@ -453,29 +402,21 @@
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 (&raw)[XYZE], const float &fr) {
#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], 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)
@ -488,11 +429,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], 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], raw[E_AXIS], fr, active_extruder);
#endif #endif
} }
@ -511,15 +452,23 @@
/** /**
* 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 (&in_target)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary if (!position_is_reachable(in_target[X_AXIS], in_target[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
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
float rtarget[XYZE] = { in_target[X_AXIS], in_target[Y_AXIS], in_target[Z_AXIS], in_target[E_AXIS] };
planner.skew(rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS]);
#else
const float (&rtarget)[XYZE] = in_target;
#endif
const float total[XYZE] = { const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS], rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS], rtarget[Y_AXIS] - current_position[Y_AXIS],
@ -564,6 +513,10 @@
current_position[E_AXIS] current_position[E_AXIS]
}; };
#if ENABLED(SKEW_CORRECTION)
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#endif
// 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(rtarget[Z_AXIS])) { // no mesh leveling if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) { while (--segments) {
@ -670,6 +623,6 @@
} // cell loop } // cell loop
} }
#endif // UBL_DELTA #endif // UBL_SEGMENTED
#endif // AUTO_BED_LEVELING_UBL #endif // AUTO_BED_LEVELING_UBL

@ -2755,7 +2755,6 @@ void kill_screen(const char* lcd_msg) {
#if IS_KINEMATIC #if IS_KINEMATIC
extern float feedrate_mm_s; extern float feedrate_mm_s;
extern float destination[XYZE];
void set_destination_from_current(); void set_destination_from_current();
void prepare_move_to_destination(); void prepare_move_to_destination();
#endif #endif

Loading…
Cancel
Save