From 8244284116cd03a9f31b97df720f844afac234b1 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 03:24:44 -0600 Subject: [PATCH 1/9] _buffer_line => buffer_segment --- Marlin/planner.cpp | 8 ++++---- Marlin/planner.h | 12 ++++++------ Marlin/ubl_motion.cpp | 26 +++++++++++++------------- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index ffed52c04..1d6ab073f 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1354,7 +1354,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const } // _buffer_steps() /** - * Planner::_buffer_line + * Planner::buffer_segment * * Add a new linear movement to the buffer in axis units. * @@ -1364,7 +1364,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const * fr_mm_s - (target) speed of the move * 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 #if ENABLED(DISTINCT_E_FACTORS) if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) { @@ -1383,7 +1383,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const }; /* <-- add a slash to enable - SERIAL_ECHOPAIR(" _buffer_line FR:", fr_mm_s); + SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s); #if IS_KINEMATIC SERIAL_ECHOPAIR(" A:", a); SERIAL_ECHOPAIR(" (", position[A_AXIS]); @@ -1430,7 +1430,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const stepper.wake_up(); -} // _buffer_line() +} // buffer_segment() /** * Directly set the planner XYZ position (and stepper positions) diff --git a/Marlin/planner.h b/Marlin/planner.h index 0378e1375..24e5cbee8 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -142,7 +142,7 @@ class Planner { * head!=tail : blocks are in the buffer * 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 */ static block_t block_buffer[BLOCK_BUFFER_SIZE]; @@ -375,7 +375,7 @@ class Planner { 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. * @@ -385,7 +385,7 @@ class Planner { * fr_mm_s - (target) speed of the move * 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); @@ -405,7 +405,7 @@ class Planner { #if PLANNER_LEVELING && IS_CARTESIAN apply_leveling(rx, ry, rz); #endif - _buffer_line(rx, ry, rz, e, fr_mm_s, extruder); + buffer_segment(rx, ry, rz, e, fr_mm_s, extruder); } /** @@ -426,9 +426,9 @@ class Planner { #endif #if IS_KINEMATIC 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 - _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 } diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index a8fa65eec..bb455ab9c 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -134,7 +134,7 @@ // Note: There is no Z Correction in this case. We are off the grid and don't know what // 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(); if (g26_debug_flag) @@ -178,7 +178,7 @@ */ 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) debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); @@ -269,7 +269,7 @@ * 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 * 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 (!inf_normalized_flag) { @@ -282,7 +282,7 @@ 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 "); } @@ -333,7 +333,7 @@ * 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 * 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 (!inf_normalized_flag) { @@ -346,7 +346,7 @@ 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 "); } @@ -408,7 +408,7 @@ e_position = end[E_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; yi_cnt--; } @@ -436,7 +436,7 @@ 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; xi_cnt--; } @@ -468,14 +468,14 @@ #endif // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, - // so we call _buffer_line directly here. Per-segmented leveling 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) { #if ENABLED(DELTA) // apply delta inverse_kinematics 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) @@ -488,11 +488,11 @@ scara_oldB = delta[B_AXIS]; float s_feedrate = max(adiff, bdiff) * scara_feed_factor; - planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], 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 - 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 } @@ -511,7 +511,7 @@ /** * 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). */ From da2eaa6b09fae45ca5dcbd12dc928d5dd721ce2f Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 05:11:22 -0600 Subject: [PATCH 2/9] Use array refs where possible --- Marlin/Marlin_main.cpp | 32 ++++++++++++++++---------------- Marlin/planner.cpp | 14 +++++++------- Marlin/planner.h | 8 ++++---- Marlin/ubl.h | 2 +- Marlin/ubl_motion.cpp | 4 ++-- 5 files changed, 30 insertions(+), 30 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 5df216651..3ed0d723e 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -733,11 +733,11 @@ void get_cartesian_from_steppers(); void set_current_from_steppers_for_axis(const AxisEnum axis); #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 #if ENABLED(BEZIER_CURVE_SUPPORT) - void plan_cubic_move(const float offset[4]); + void plan_cubic_move(const float (&offset)[4]); #endif void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false); @@ -1808,7 +1808,7 @@ static void clean_up_after_endstop_or_probe_move() { #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); } @@ -8326,7 +8326,7 @@ void report_current_position() { #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]; for (uint8_t i = 0; i < n; i++) { SERIAL_CHAR(' '); @@ -8337,7 +8337,7 @@ void report_current_position() { 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() { @@ -12659,7 +12659,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { * For Unified Bed Leveling (Delta or Segmented Cartesian) * 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 const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s); @@ -12968,9 +12968,9 @@ void prepare_move_to_destination() { * options for G2/G3 arc generation. In future these options may be GCode tunable. */ void plan_arc( - float raw[XYZE], // Destination position - float *offset, // Center of rotation relative to current_position - uint8_t clockwise // Clockwise? + const float (&cart)[XYZE], // Destination position + const float (&offset)[2], // Center of rotation relative to current_position + const bool clockwise // Clockwise? ) { #if ENABLED(CNC_WORKSPACE_PLANES) AxisEnum p_axis, q_axis, l_axis; @@ -12990,10 +12990,10 @@ void prepare_move_to_destination() { const float radius = HYPOT(r_P, r_Q), center_P = current_position[p_axis] - r_P, center_Q = current_position[q_axis] - r_Q, - rt_X = raw[p_axis] - center_P, - rt_Y = raw[q_axis] - center_Q, - linear_travel = raw[l_axis] - current_position[l_axis], - extruder_travel = raw[E_AXIS] - current_position[E_AXIS]; + rt_X = cart[p_axis] - center_P, + rt_Y = cart[q_axis] - center_Q, + linear_travel = cart[l_axis] - current_position[l_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. float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y); @@ -13001,7 +13001,7 @@ void prepare_move_to_destination() { if (clockwise) angular_travel -= RADIANS(360); // 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); const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel)); @@ -13101,7 +13101,7 @@ void prepare_move_to_destination() { } // 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 // motion control system might still be processing the action and the real tool position @@ -13113,7 +13113,7 @@ void prepare_move_to_destination() { #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); // As far as the parser is concerned, the position is now == destination. In reality the diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 1d6ab073f..5287d5413 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1455,18 +1455,18 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c 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 - float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] }; - apply_leveling(lpos); + float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; + apply_leveling(raw); #else - const float * const lpos = position; + const float (&raw)[XYZE] = cart; #endif #if IS_KINEMATIC - inverse_kinematics(lpos); - _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]); + inverse_kinematics(raw); + _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]); #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 } diff --git a/Marlin/planner.h b/Marlin/planner.h index 24e5cbee8..a30aaacf2 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -352,7 +352,7 @@ class Planner { * 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 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]); #else @@ -417,12 +417,12 @@ class Planner { * fr_mm_s - (target) speed of the move (mm/s) * 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 float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; apply_leveling(raw); #else - const float * const raw = cart; + const float (&raw)[XYZE] = cart; #endif #if IS_KINEMATIC inverse_kinematics(raw); @@ -447,7 +447,7 @@ class Planner { #endif _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); 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); } diff --git a/Marlin/ubl.h b/Marlin/ubl.h index b1d8ba3ea..05a22392c 100644 --- a/Marlin/ubl.h +++ b/Marlin/ubl.h @@ -319,7 +319,7 @@ 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); + static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate); static void line_to_destination_cartesian(const float &fr, uint8_t e); #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1]) diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index bb455ab9c..feded987d 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -470,7 +470,7 @@ // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, // 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 @@ -515,7 +515,7 @@ * 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 return true; // did not move, so current_position still accurate From bb33a26e6226cfc3484bd798aa560c4661c1397b Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 06:43:38 -0600 Subject: [PATCH 3/9] Apply const to axis args --- Marlin/Marlin_main.cpp | 2 +- Marlin/stepper.cpp | 4 ++-- Marlin/stepper.h | 12 ++++++------ Marlin/stepper_dac.cpp | 2 +- Marlin/stepper_dac.h | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 3ed0d723e..49c6d3b3c 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -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) \ 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##__ XYZ_CONSTS_FROM_CONFIG(float, base_min_pos, MIN_POS); diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index bab1f5cbc..afc2f7b29 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -1198,7 +1198,7 @@ void Stepper::set_e_position(const long &e) { /** * Get a stepper's position in steps. */ -long Stepper::position(AxisEnum axis) { +long Stepper::position(const AxisEnum axis) { CRITICAL_SECTION_START; const long count_pos = count_position[axis]; CRITICAL_SECTION_END; @@ -1209,7 +1209,7 @@ long Stepper::position(AxisEnum axis) { * Get an axis position according to stepper position(s) * 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; #if IS_CORE // Requesting one of the "core" axes? diff --git a/Marlin/stepper.h b/Marlin/stepper.h index c46bd052d..63aa4e6d2 100644 --- a/Marlin/stepper.h +++ b/Marlin/stepper.h @@ -209,7 +209,7 @@ class Stepper { // // 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 @@ -219,13 +219,13 @@ class Stepper { // // 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 // #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 // @@ -247,7 +247,7 @@ class Stepper { // // 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 static void digitalPotWrite(const int16_t address, const int16_t value); @@ -287,12 +287,12 @@ class Stepper { // // 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) // - 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]; } diff --git a/Marlin/stepper_dac.cpp b/Marlin/stepper_dac.cpp index 6ea8b83bc..f7161e346 100644 --- a/Marlin/stepper_dac.cpp +++ b/Marlin/stepper_dac.cpp @@ -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_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]) { LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]]; mcp4728_setDrvPct(dac_channel_pct); diff --git a/Marlin/stepper_dac.h b/Marlin/stepper_dac.h index 588035040..cf56050e7 100644 --- a/Marlin/stepper_dac.h +++ b/Marlin/stepper_dac.h @@ -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_print_values(); 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]); #endif // STEPPER_DAC_H From 6e8da93c4225b736ea96266a37468006038a8e46 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 03:26:48 -0600 Subject: [PATCH 4/9] UBL_DELTA => UBL_SEGMENTED --- Marlin/Conditionals_post.h | 18 +++++++++--------- Marlin/G26_Mesh_Validation_Tool.cpp | 2 +- Marlin/Marlin_main.cpp | 8 ++++---- Marlin/SanityCheck.h | 2 +- Marlin/planner.cpp | 2 +- Marlin/ubl_motion.cpp | 4 ++-- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 42b8e342c..6f902a301 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -933,15 +933,15 @@ /** * 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 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 OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID) - #define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL)) - #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) - #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 PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA || ENABLED(SKEW_CORRECTION)) + #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_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR)) + #define OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID) + #define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL)) + #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) + #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 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)) #if HAS_PROBING_PROCEDURE #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION)) diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index 1abe88e60..7e1f73960 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -189,7 +189,7 @@ void G26_line_to_destination(const float &feed_rate) { const float save_feedrate = feedrate_mm_s; 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 } diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 49c6d3b3c..fe7cb211c 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1550,7 +1550,7 @@ inline void set_destination_from_current() { COPY(destination, current_position) refresh_cmd_timeout(); - #if UBL_DELTA + #if UBL_SEGMENTED // ubl segmented line will do z-only moves in single segment ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s)); #else @@ -12647,7 +12647,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { #endif // AUTO_BED_LEVELING_BILINEAR #endif // IS_CARTESIAN -#if !UBL_DELTA +#if !UBL_SEGMENTED #if IS_KINEMATIC /** @@ -12819,7 +12819,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { } #endif // !IS_KINEMATIC -#endif // !UBL_DELTA +#endif // !UBL_SEGMENTED #if ENABLED(DUAL_X_CARRIAGE) @@ -12937,7 +12937,7 @@ void prepare_move_to_destination() { #endif if ( - #if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely) + #if UBL_SEGMENTED // Also works for CARTESIAN (smaller segments follow mesh more closely) ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s)) #elif IS_KINEMATIC prepare_kinematic_move_to(destination) diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index 1f9dccc36..e5ddf6b83 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -590,7 +590,7 @@ static_assert(1 >= 0 #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) #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." #elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL)) #error "DELTA_AUTO_CALIBRATION requires either a probe or an LCD Controller." diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 5287d5413..d566982fe 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -605,7 +605,7 @@ void Planner::calculate_volumetric_multipliers() { #endif rz += ( - #if ENABLED(AUTO_BED_LEVELING_UBL) // UBL_DELTA + #if ENABLED(AUTO_BED_LEVELING_UBL) ubl.get_z_correction(rx, ry) * fade_scaling_factor #elif ENABLED(MESH_BED_LEVELING) mbl.get_z(rx, ry diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index feded987d..3b0c9dc79 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -453,7 +453,7 @@ set_current_from_destination(); } - #if UBL_DELTA + #if UBL_SEGMENTED // macro to inline copy exactly 4 floats, don't rely on sizeof operator #define COPY_XYZE( target, source ) { \ @@ -670,6 +670,6 @@ } // cell loop } - #endif // UBL_DELTA + #endif // UBL_SEGMENTED #endif // AUTO_BED_LEVELING_UBL From 4f042533a50511ce6ca65c850c91634a8b07ce11 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 05:03:54 -0600 Subject: [PATCH 5/9] Fix DUAL_X_CARRIAGE with UBL segmented --- Marlin/Marlin_main.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index fe7cb211c..240d192ed 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -12895,7 +12895,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { 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 @@ -12937,12 +12943,12 @@ void prepare_move_to_destination() { #endif if ( - #if UBL_SEGMENTED // 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)) #elif IS_KINEMATIC prepare_kinematic_move_to(destination) - #elif ENABLED(DUAL_X_CARRIAGE) - prepare_move_to_destination_dualx() #else prepare_move_to_destination_cartesian() #endif From a20eacaa4868e21cccd025ce1d85fcce2e3d979b Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 03:58:12 -0600 Subject: [PATCH 6/9] UBL devel debugging flag --- Marlin/G26_Mesh_Validation_Tool.cpp | 1 - Marlin/Marlin.h | 2 +- Marlin/ubl.cpp | 55 ++++++++++++++++++++++++++++- Marlin/ubl.h | 9 ++++- Marlin/ubl_G29.cpp | 10 +++--- Marlin/ubl_motion.cpp | 51 -------------------------- Marlin/ultralcd.cpp | 1 - 7 files changed, 67 insertions(+), 62 deletions(-) diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index 7e1f73960..e8ca670f0 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -137,7 +137,6 @@ #if ENABLED(ULTRA_LCD) extern char lcd_status_message[]; #endif - extern float destination[XYZE]; void set_destination_from_current(); void prepare_move_to_destination(); inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index 7c0abe69d..eb10f08c3 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -223,7 +223,7 @@ extern volatile bool wait_for_heatup; extern volatile bool wait_for_user; #endif -extern float current_position[NUM_AXIS]; +extern float current_position[XYZE], destination[XYZE]; // Workspace offsets #if HAS_WORKSPACE_OFFSET diff --git a/Marlin/ubl.cpp b/Marlin/ubl.cpp index bbf51b7e2..11a2a2223 100644 --- a/Marlin/ubl.cpp +++ b/Marlin/ubl.cpp @@ -51,6 +51,59 @@ 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; float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; @@ -174,7 +227,7 @@ uint8_t error_flag = 0; 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++; } diff --git a/Marlin/ubl.h b/Marlin/ubl.h index 05a22392c..f7c0951ea 100644 --- a/Marlin/ubl.h +++ b/Marlin/ubl.h @@ -26,6 +26,9 @@ #include "MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) + + //#define UBL_DEVEL_DEBUGGING + #include "Marlin.h" #include "planner.h" #include "math.h" @@ -41,7 +44,11 @@ // 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 diff --git a/Marlin/ubl_G29.cpp b/Marlin/ubl_G29.cpp index 8800c6e9a..6b56ffafc 100644 --- a/Marlin/ubl_G29.cpp +++ b/Marlin/ubl_G29.cpp @@ -24,8 +24,6 @@ #if ENABLED(AUTO_BED_LEVELING_UBL) - //#define UBL_DEVEL_DEBUGGING - #include "ubl.h" #include "Marlin.h" #include "hex_print_routines.h" @@ -1165,12 +1163,12 @@ 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; #endif void unified_bed_leveling::save_ubl_active_state_and_disable() { - #ifdef UBL_DEVEL_DEBUGGING + #if ENABLED(UBL_DEVEL_DEBUGGING) ubl_state_recursion_chk++; if (ubl_state_recursion_chk != 1) { 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() { - #ifdef UBL_DEVEL_DEBUGGING + #if ENABLED(UBL_DEVEL_DEBUGGING) if (--ubl_state_recursion_chk) { SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times."); #if ENABLED(NEWPANEL) @@ -1267,7 +1265,7 @@ SERIAL_EOL(); safe_delay(50); - #ifdef UBL_DEVEL_DEBUGGING + #if ENABLED(UBL_DEVEL_DEBUGGING) SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation); SERIAL_EOL(); SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk); diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 3b0c9dc79..ec731d1fd 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -30,63 +30,12 @@ #include #include - extern float destination[XYZE]; - #if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this inline void set_current_from_destination() { COPY(current_position, destination); } #else extern void set_current_from_destination(); #endif - 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(); - - } - 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 diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index a20a7b37f..6205ed130 100644 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -2755,7 +2755,6 @@ void kill_screen(const char* lcd_msg) { #if IS_KINEMATIC extern float feedrate_mm_s; - extern float destination[XYZE]; void set_destination_from_current(); void prepare_move_to_destination(); #endif From 5cce532a29658024eafe417ca2e9d96165458b7f Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 03:34:41 -0600 Subject: [PATCH 7/9] One or the other? --- Marlin/ubl.h | 33 +-- Marlin/ubl_motion.cpp | 606 +++++++++++++++++++++--------------------- 2 files changed, 318 insertions(+), 321 deletions(-) diff --git a/Marlin/ubl.h b/Marlin/ubl.h index f7c0951ea..754e387dc 100644 --- a/Marlin/ubl.h +++ b/Marlin/ubl.h @@ -326,21 +326,24 @@ 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); - static void line_to_destination_cartesian(const float &fr, uint8_t e); - - #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1]) - #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1)) - #define ZZER(a) (z_values[a][0] == 0) - - FORCE_INLINE bool mesh_is_valid() { - return !( - ( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal? - && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge? - ) - || isnan(z_values[0][0]) - ); - } + #if UBL_SEGMENTED + 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) (_CMPZ(a, 0) && _CMPZ(a, 1)) + #define ZZER(a) (z_values[a][0] == 0) + + FORCE_INLINE bool mesh_is_valid() { + return !( + ( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal? + && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge? + ) + || isnan(z_values[0][0]) + ); + } }; // class unified_bed_leveling extern unified_bed_leveling ubl; diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index ec731d1fd..2681cbeb5 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -36,193 +36,322 @@ extern void set_current_from_destination(); #endif - 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 - * 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 - */ - const float start[XYZE] = { - current_position[X_AXIS], - current_position[Y_AXIS], - current_position[Z_AXIS], - current_position[E_AXIS] - }, - end[XYZE] = { - destination[X_AXIS], - destination[Y_AXIS], - destination[Z_AXIS], - destination[E_AXIS] - }; - - const int cell_start_xi = get_cell_index_x(start[X_AXIS]), - cell_start_yi = get_cell_index_y(start[Y_AXIS]), - cell_dest_xi = get_cell_index_x(end[X_AXIS]), - cell_dest_yi = get_cell_index_y(end[Y_AXIS]); - - if (g26_debug_flag) { - SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]); - SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); - SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); - SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); - SERIAL_CHAR(')'); - SERIAL_EOL(); - debug_current_and_destination(PSTR("Start of ubl.line_to_destination()")); - } + #if !UBL_SEGMENTED - if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell, + void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) { /** - * we don't need to break up the move - * - * If we are moving off the print bed, we are going to allow the move at this level. - * But we detect it and isolate it. For now, we just pass along the request. + * 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 + * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave */ + const float start[XYZE] = { + current_position[X_AXIS], + current_position[Y_AXIS], + current_position[Z_AXIS], + current_position[E_AXIS] + }, + end[XYZE] = { + destination[X_AXIS], + destination[Y_AXIS], + destination[Z_AXIS], + destination[E_AXIS] + }; + + const int cell_start_xi = get_cell_index_x(start[X_AXIS]), + cell_start_yi = get_cell_index_y(start[Y_AXIS]), + cell_dest_xi = get_cell_index_x(end[X_AXIS]), + cell_dest_yi = get_cell_index_y(end[Y_AXIS]); + + if (g26_debug_flag) { + SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]); + SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); + SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); + SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); + SERIAL_CHAR(')'); + SERIAL_EOL(); + debug_current_and_destination(PSTR("Start of ubl.line_to_destination()")); + } - if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) { + if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell, + /** + * we don't need to break up the move + * + * If we are moving off the print bed, we are going to allow the move at this level. + * But we detect it and isolate it. For now, we just pass along the request. + */ - // Note: There is no Z Correction in this case. We are off the grid and don't know what - // a reasonable correction would be. + if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) { - planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder); - set_current_from_destination(); + // Note: There is no Z Correction in this case. We are off the grid and don't know what + // a reasonable correction would be. + + planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder); + set_current_from_destination(); + + if (g26_debug_flag) + debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()")); + + return; + } + + FINAL_MOVE: + + /** + * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to + * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function. + * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once. + * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And, + * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor + * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. + */ + + const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)); + + float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio * + (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]), + z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio * + (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]); + + if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0; + + // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we + // are going to apply the Y-Distance into the cell to interpolate the final Z correction. + + const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST)); + float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0; + + /** + * If part of the Mesh is undefined, it will show up as NAN + * in z_values[][] and propagate through the + * calculations. If our correction is NAN, we throw it out + * because part of the Mesh is undefined and we don't have the + * information we need to complete the height correction. + */ + if (isnan(z0)) z0 = 0.0; + + planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder); if (g26_debug_flag) - debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); + set_current_from_destination(); return; } - FINAL_MOVE: + /** + * If we get here, we are processing a move that crosses at least one Mesh Line. We will check + * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details + * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less + * computation and in fact most lines are of this nature. We will check for that in the following + * blocks of code: + */ + + const float dx = end[X_AXIS] - start[X_AXIS], + dy = end[Y_AXIS] - start[Y_AXIS]; + + const int left_flag = dx < 0.0 ? 1 : 0, + down_flag = dy < 0.0 ? 1 : 0; + + const float adx = left_flag ? -dx : dx, + ady = down_flag ? -dy : dy; + + const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1, + dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1; /** - * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to - * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function. - * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once. - * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And, - * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor - * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide. + * Compute the scaling factor for the extruder for each partial move. + * We need to watch out for zero length moves because it will cause us to + * have an infinate scaling factor. We are stuck doing a floating point + * divide to get our scaling factor, but after that, we just multiply by this + * number. We also pick our scaling factor based on whether the X or Y + * component is larger. We use the biggest of the two to preserve precision. */ - const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)); + const bool use_x_dist = adx > ady; - float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio * - (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]), - z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio * - (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]); + float on_axis_distance = use_x_dist ? dx : dy, + e_position = end[E_AXIS] - start[E_AXIS], + z_position = end[Z_AXIS] - start[Z_AXIS]; - if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0; + const float e_normalized_dist = e_position / on_axis_distance, + z_normalized_dist = z_position / on_axis_distance; - // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we - // are going to apply the Y-Distance into the cell to interpolate the final Z correction. + int current_xi = cell_start_xi, + current_yi = cell_start_yi; - const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST)); - float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0; + const float m = dy / dx, + c = start[Y_AXIS] - m * start[X_AXIS]; + const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0), + inf_m_flag = (isinf(m) != 0); /** - * If part of the Mesh is undefined, it will show up as NAN - * in z_values[][] and propagate through the - * calculations. If our correction is NAN, we throw it out - * because part of the Mesh is undefined and we don't have the - * information we need to complete the height correction. + * This block handles vertical lines. These are lines that stay within the same + * X Cell column. They do not need to be perfectly vertical. They just can + * not cross into another X Cell column. */ - if (isnan(z0)) z0 = 0.0; - - planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder); + if (dxi == 0) { // Check for a vertical line + current_yi += down_flag; // Line is heading down, we just want to go to the bottom + while (current_yi != cell_dest_yi + down_flag) { + current_yi += dyi; + const float next_mesh_line_y = mesh_index_to_ypos(current_yi); + + /** + * if the slope of the line is infinite, we won't do the calculations + * else, we know the next X is the same so we can recover and continue! + * Calculate X at the next Y mesh line + */ + const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; + + float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi) + * planner.fade_scaling_factor_for_z(end[Z_AXIS]); + + /** + * If part of the Mesh is undefined, it will show up as NAN + * in z_values[][] and propagate through the + * calculations. If our correction is NAN, we throw it out + * because part of the Mesh is undefined and we don't have the + * information we need to complete the height correction. + */ + if (isnan(z0)) z0 = 0.0; + + const float ry = mesh_index_to_ypos(current_yi); + + /** + * Without this check, it is possible for the algorithm to generate a zero length move in the case + * 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 + * the planner.buffer_segment() routine will filter it if that happens. + */ + if (ry != start[Y_AXIS]) { + if (!inf_normalized_flag) { + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; + e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; + z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; + } + else { + e_position = end[E_AXIS]; + z_position = end[Z_AXIS]; + } - if (g26_debug_flag) - debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } - set_current_from_destination(); - return; - } + if (g26_debug_flag) + debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()")); - /** - * If we get here, we are processing a move that crosses at least one Mesh Line. We will check - * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details - * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less - * computation and in fact most lines are of this nature. We will check for that in the following - * blocks of code: - */ + // + // 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. + // + if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) + goto FINAL_MOVE; - const float dx = end[X_AXIS] - start[X_AXIS], - dy = end[Y_AXIS] - start[Y_AXIS]; + set_current_from_destination(); + return; + } - const int left_flag = dx < 0.0 ? 1 : 0, - down_flag = dy < 0.0 ? 1 : 0; + /** + * + * This block handles horizontal lines. These are lines that stay within the same + * Y Cell row. They do not need to be perfectly horizontal. They just can + * not cross into another Y Cell row. + * + */ - const float adx = left_flag ? -dx : dx, - ady = down_flag ? -dy : dy; + if (dyi == 0) { // Check for a horizontal line + current_xi += left_flag; // Line is heading left, we just want to go to the left + // edge of this cell for the first move. + while (current_xi != cell_dest_xi + left_flag) { + current_xi += dxi; + const float next_mesh_line_x = mesh_index_to_xpos(current_xi), + ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line + + float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi) + * planner.fade_scaling_factor_for_z(end[Z_AXIS]); + + /** + * If part of the Mesh is undefined, it will show up as NAN + * in z_values[][] and propagate through the + * calculations. If our correction is NAN, we throw it out + * because part of the Mesh is undefined and we don't have the + * information we need to complete the height correction. + */ + if (isnan(z0)) z0 = 0.0; + + const float rx = mesh_index_to_xpos(current_xi); + + /** + * Without this check, it is possible for the algorithm to generate a zero length move in the case + * 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 + * the planner.buffer_segment() routine will filter it if that happens. + */ + if (rx != start[X_AXIS]) { + if (!inf_normalized_flag) { + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; + e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move + z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; + } + else { + e_position = end[E_AXIS]; + z_position = end[Z_AXIS]; + } - const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1, - dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1; + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } - /** - * Compute the scaling factor for the extruder for each partial move. - * We need to watch out for zero length moves because it will cause us to - * have an infinate scaling factor. We are stuck doing a floating point - * divide to get our scaling factor, but after that, we just multiply by this - * number. We also pick our scaling factor based on whether the X or Y - * component is larger. We use the biggest of the two to preserve precision. - */ + if (g26_debug_flag) + debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()")); - const bool use_x_dist = adx > ady; + if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) + goto FINAL_MOVE; - float on_axis_distance = use_x_dist ? dx : dy, - e_position = end[E_AXIS] - start[E_AXIS], - z_position = end[Z_AXIS] - start[Z_AXIS]; + set_current_from_destination(); + return; + } - const float e_normalized_dist = e_position / on_axis_distance, - z_normalized_dist = z_position / on_axis_distance; + /** + * + * This block handles the generic case of a line crossing both X and Y Mesh lines. + * + */ - int current_xi = cell_start_xi, - current_yi = cell_start_yi; + int xi_cnt = cell_start_xi - cell_dest_xi, + yi_cnt = cell_start_yi - cell_dest_yi; - const float m = dy / dx, - c = start[Y_AXIS] - m * start[X_AXIS]; + if (xi_cnt < 0) xi_cnt = -xi_cnt; + if (yi_cnt < 0) yi_cnt = -yi_cnt; - const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0), - inf_m_flag = (isinf(m) != 0); - /** - * This block handles vertical lines. These are lines that stay within the same - * X Cell column. They do not need to be perfectly vertical. They just can - * not cross into another X Cell column. - */ - if (dxi == 0) { // Check for a vertical line - current_yi += down_flag; // Line is heading down, we just want to go to the bottom - while (current_yi != cell_dest_yi + down_flag) { - current_yi += dyi; - const float next_mesh_line_y = mesh_index_to_ypos(current_yi); + current_xi += left_flag; + current_yi += down_flag; - /** - * if the slope of the line is infinite, we won't do the calculations - * else, we know the next X is the same so we can recover and continue! - * Calculate X at the next Y mesh line - */ - const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; + while (xi_cnt > 0 || yi_cnt > 0) { - float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi) - * planner.fade_scaling_factor_for_z(end[Z_AXIS]); + const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi), + next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi), + ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line + rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line + // (No need to worry about m being zero. + // If that was the case, it was already detected + // as a vertical line move above.) - /** - * If part of the Mesh is undefined, it will show up as NAN - * in z_values[][] and propagate through the - * calculations. If our correction is NAN, we throw it out - * because part of the Mesh is undefined and we don't have the - * information we need to complete the height correction. - */ - if (isnan(z0)) z0 = 0.0; + if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first + // Yes! Crossing a Y Mesh Line next + float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi) + * planner.fade_scaling_factor_for_z(end[Z_AXIS]); - const float ry = mesh_index_to_ypos(current_yi); + /** + * If part of the Mesh is undefined, it will show up as NAN + * in z_values[][] and propagate through the + * calculations. If our correction is NAN, we throw it out + * because part of the Mesh is undefined and we don't have the + * information we need to complete the height correction. + */ + if (isnan(z0)) z0 = 0.0; - /** - * 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 - * happens, it might be best to remove the check and always 'schedule' the move because - * the planner.buffer_segment() routine will filter it if that happens. - */ - if (ry != start[Y_AXIS]) { if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS]; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } @@ -230,64 +359,27 @@ e_position = end[E_AXIS]; z_position = end[Z_AXIS]; } + planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); + current_yi += dyi; + yi_cnt--; + } + else { + // Yes! Crossing a X Mesh Line next + float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag) + * planner.fade_scaling_factor_for_z(end[Z_AXIS]); + + /** + * If part of the Mesh is undefined, it will show up as NAN + * in z_values[][] and propagate through the + * calculations. If our correction is NAN, we throw it out + * because part of the Mesh is undefined and we don't have the + * information we need to complete the height correction. + */ + if (isnan(z0)) z0 = 0.0; - planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); - } //else printf("FIRST MOVE PRUNED "); - } - - if (g26_debug_flag) - debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()")); - - // - // 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. - // - if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) - goto FINAL_MOVE; - - set_current_from_destination(); - return; - } - - /** - * - * This block handles horizontal lines. These are lines that stay within the same - * Y Cell row. They do not need to be perfectly horizontal. They just can - * not cross into another Y Cell row. - * - */ - - if (dyi == 0) { // Check for a horizontal line - current_xi += left_flag; // Line is heading left, we just want to go to the left - // edge of this cell for the first move. - while (current_xi != cell_dest_xi + left_flag) { - current_xi += dxi; - const float next_mesh_line_x = mesh_index_to_xpos(current_xi), - ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line - - float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi) - * planner.fade_scaling_factor_for_z(end[Z_AXIS]); - - /** - * If part of the Mesh is undefined, it will show up as NAN - * in z_values[][] and propagate through the - * calculations. If our correction is NAN, we throw it out - * because part of the Mesh is undefined and we don't have the - * information we need to complete the height correction. - */ - if (isnan(z0)) z0 = 0.0; - - const float rx = mesh_index_to_xpos(current_xi); - - /** - * Without this check, it is possible for the algorithm to generate a zero length move in the case - * 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 - * the planner.buffer_segment() routine will filter it if that happens. - */ - if (rx != start[X_AXIS]) { if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; - e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move + on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS]; + e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } else { @@ -295,122 +387,24 @@ z_position = end[Z_AXIS]; } - planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); - } //else printf("FIRST MOVE PRUNED "); + planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); + current_xi += dxi; + xi_cnt--; + } + + if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE } if (g26_debug_flag) - debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()")); if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) goto FINAL_MOVE; set_current_from_destination(); - return; } - /** - * - * This block handles the generic case of a line crossing both X and Y Mesh lines. - * - */ - - int xi_cnt = cell_start_xi - cell_dest_xi, - yi_cnt = cell_start_yi - cell_dest_yi; - - if (xi_cnt < 0) xi_cnt = -xi_cnt; - if (yi_cnt < 0) yi_cnt = -yi_cnt; - - current_xi += left_flag; - current_yi += down_flag; - - while (xi_cnt > 0 || yi_cnt > 0) { - - const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi), - next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi), - ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line - rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line - // (No need to worry about m being zero. - // If that was the case, it was already detected - // as a vertical line move above.) - - if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first - // Yes! Crossing a Y Mesh Line next - float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi) - * planner.fade_scaling_factor_for_z(end[Z_AXIS]); - - /** - * If part of the Mesh is undefined, it will show up as NAN - * in z_values[][] and propagate through the - * calculations. If our correction is NAN, we throw it out - * because part of the Mesh is undefined and we don't have the - * information we need to complete the height correction. - */ - if (isnan(z0)) z0 = 0.0; - - if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS]; - e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; - z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; - } - else { - e_position = end[E_AXIS]; - z_position = end[Z_AXIS]; - } - planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); - current_yi += dyi; - yi_cnt--; - } - else { - // Yes! Crossing a X Mesh Line next - float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag) - * planner.fade_scaling_factor_for_z(end[Z_AXIS]); - - /** - * If part of the Mesh is undefined, it will show up as NAN - * in z_values[][] and propagate through the - * calculations. If our correction is NAN, we throw it out - * because part of the Mesh is undefined and we don't have the - * information we need to complete the height correction. - */ - if (isnan(z0)) z0 = 0.0; - - if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS]; - e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; - z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; - } - else { - e_position = end[E_AXIS]; - z_position = end[Z_AXIS]; - } - - planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); - current_xi += dxi; - xi_cnt--; - } - - if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE - } - - if (g26_debug_flag) - debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()")); - - if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) - goto FINAL_MOVE; - - set_current_from_destination(); - } - - #if 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]; \ - } + #else // UBL_SEGMENTED #if IS_SCARA // scale the feed rate from mm/s to degrees/s static float scara_feed_factor, scara_oldA, scara_oldB; From ba48ce85862f180e5915ab77c1bf428c891fdb4e Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 03:42:47 -0600 Subject: [PATCH 8/9] Travis Test for non-segmented UBL --- .travis.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index b14b43d9c..be830d0b3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -103,9 +103,10 @@ script: - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING NANODLP_Z_SYNC - 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 - build_marlin # @@ -141,7 +142,7 @@ script: - opt_enable ULTIMAKERCONTROLLER SDSUPPORT - opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG - 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 I2C_SLAVE_ADDRESS 63 - build_marlin From bdf69db0a802ab0f2c6b2bd579ffc1182fabe6c0 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 04:55:02 -0600 Subject: [PATCH 9/9] Extend Skew Correction to UBL --- Marlin/SanityCheck.h | 3 --- Marlin/planner.cpp | 18 ++------------ Marlin/planner.h | 24 +++++++++++++++++++ Marlin/ubl_motion.cpp | 56 +++++++++++++++++++++++++------------------ 4 files changed, 59 insertions(+), 42 deletions(-) diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index e5ddf6b83..75d939f94 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -1539,9 +1539,6 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m #endif #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)) #error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD." #endif diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index d566982fe..67cf0b391 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -569,14 +569,7 @@ void Planner::calculate_volumetric_multipliers() { void Planner::apply_leveling(float &rx, float &ry, float &rz) { #if ENABLED(SKEW_CORRECTION) - if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) { - 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; - } - } + skew(rx, ry, rz); #endif if (!leveling_active) return; @@ -667,14 +660,7 @@ void Planner::calculate_volumetric_multipliers() { } #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)) { - 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; - } - } + unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); #endif } diff --git a/Marlin/planner.h b/Marlin/planner.h index a30aaacf2..631ccf400 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -341,6 +341,30 @@ class Planner { #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 #define ARG_X float rx diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 2681cbeb5..880e6d6cb 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -44,18 +44,16 @@ * 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 */ - const float start[XYZE] = { - current_position[X_AXIS], - current_position[Y_AXIS], - current_position[Z_AXIS], - current_position[E_AXIS] - }, - end[XYZE] = { - destination[X_AXIS], - destination[Y_AXIS], - destination[Z_AXIS], - destination[E_AXIS] - }; + #if ENABLED(SKEW_CORRECTION) + // For skew correction just adjust the destination point and we're done + float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] }, + end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] }; + planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]); + planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]); + #else + const float (&start)[XYZE] = current_position, + (&end)[XYZE] = destination; + #endif const int cell_start_xi = get_cell_index_x(start[X_AXIS]), cell_start_yi = get_cell_index_y(start[Y_AXIS]), @@ -63,13 +61,13 @@ cell_dest_yi = get_cell_index_y(end[Y_AXIS]); if (g26_debug_flag) { - SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]); - SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]); - SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]); - SERIAL_ECHOPAIR(", ee=", end[E_AXIS]); + SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]); + SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]); + SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]); + SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]); SERIAL_CHAR(')'); 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, @@ -89,7 +87,7 @@ set_current_from_destination(); 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; } @@ -132,7 +130,7 @@ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder); 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(); return; @@ -238,7 +236,7 @@ } 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. @@ -302,7 +300,7 @@ } 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]) goto FINAL_MOVE; @@ -396,7 +394,7 @@ } 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]) goto FINAL_MOVE; @@ -460,9 +458,17 @@ 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 + #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] = { rtarget[X_AXIS] - current_position[X_AXIS], rtarget[Y_AXIS] - current_position[Y_AXIS], @@ -507,6 +513,10 @@ 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. if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling while (--segments) {