From 23880873a04f9c9592ef87ba9bb2ccb07d94af8c Mon Sep 17 00:00:00 2001 From: DerAndere <26200979+DerAndere1@users.noreply.github.com> Date: Tue, 21 Feb 2023 19:26:10 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20Add=20get?= =?UTF-8?q?=5Fmove=5Fdistance=20for=20rotation/kinematics=20(#25370)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/module/motion.cpp | 103 +++++++++++++++++++++++++++++++++- Marlin/src/module/motion.h | 2 + Marlin/src/module/planner.cpp | 102 +++++++++++---------------------- Marlin/src/module/planner.h | 5 ++ 4 files changed, 139 insertions(+), 73 deletions(-) diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index b32368ca92d3e..9d43db48c74a6 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -1059,6 +1059,88 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { thermalManager.task(); // Returns immediately on most calls } +/** + * Get distance from displacements along axes and, if required, update move type. + */ +float get_move_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool &is_cartesian_move)) { + if (!(NUM_AXIS_GANG(diff.x, || diff.y, /* skip z */, || diff.i, || diff.j, || diff.k, || diff.u, || diff.v, || diff.w))) + return TERN0(HAS_Z_AXIS, ABS(diff.z)); + + #if ENABLED(ARTICULATED_ROBOT_ARM) + + // For articulated robots, interpreting feedrate like LinuxCNC would require inverse kinematics. As a workaround, pretend that motors sit on n mutually orthogonal + // axes and assume that we could think of distance as magnitude of an n-vector in an n-dimensional Euclidian space. + const float distance_sqr = NUM_AXIS_GANG( + sq(diff.x), + sq(diff.y), + sq(diff.z), + + sq(diff.i), + sq(diff.j), + sq(diff.k), + + sq(diff.u), + sq(diff.v), + sq(diff.w) + ); + + #elif ENABLED(FOAMCUTTER_XYUV) + + const float distance_sqr = ( + #if HAS_J_AXIS + _MAX(sq(diff.x) + sq(diff.y), sq(diff.i) + sq(diff.j)) // Special 5 axis kinematics. Return the larger of plane X/Y or I/J + #else + sq(diff.x) + sq(diff.y) // Foamcutter with only two axes (XY) + #endif + ); + + #else + + /** + * Calculate distance for feedrate interpretation in accordance with NIST RS274NGC interpreter - version 3) and its default CANON_XYZ feed reference mode. + * Assume: + * - X, Y, Z are the primary linear axes; + * - U, V, W are secondary linear axes; + * - A, B, C are rotational axes. + * + * Then: + * - dX, dY, dZ are the displacements of the primary linear axes; + * - dU, dV, dW are the displacements of linear axes; + * - dA, dB, dC are the displacements of rotational axes. + * + * The time it takes to execute a move command with feedrate F is t = D/F, + * plus any time for acceleration and deceleration. + * Here, D is the total distance, calculated as follows: + * + * D^2 = dX^2 + dY^2 + dZ^2 + * if D^2 == 0 (none of XYZ move but any secondary linear axes move, whether other axes are moved or not): + * D^2 = dU^2 + dV^2 + dW^2 + * if D^2 == 0 (only rotational axes are moved): + * D^2 = dA^2 + dB^2 + dC^2 + */ + float distance_sqr = XYZ_GANG(sq(diff.x), + sq(diff.y), + sq(diff.z)); + + #if SECONDARY_LINEAR_AXES + if (UNEAR_ZERO(distance_sqr)) { + // Move does not involve any primary linear axes (xyz) but might involve secondary linear axes + distance_sqr = ( + SECONDARY_AXIS_GANG( + IF_DISABLED(AXIS4_ROTATES, + sq(diff.i)), + IF_DISABLED(AXIS5_ROTATES, + sq(diff.j)), + IF_DISABLED(AXIS6_ROTATES, + sq(diff.k)), + IF_DISABLED(AXIS7_ROTATES, + sq(diff.u)), + IF_DISABLED(AXIS8_ROTATES, + sq(diff.v)), + IF_DISABLED(AXIS9_ROTATES, + sq(diff.w)) + ) + ); + } + #endif + + #if HAS_ROTATIONAL_AXES + if (UNEAR_ZERO(distance_sqr)) { + // Move involves only rotational axes. Calculate angular distance in accordance with LinuxCNC + is_cartesian_move = false; + distance_sqr = ROTATIONAL_AXIS_GANG(sq(diff.i), + sq(diff.j), + sq(diff.k), + sq(diff.u), + sq(diff.v), + sq(diff.w)); + } + #endif + + #endif + + return SQRT(distance_sqr); +} + #if IS_KINEMATIC #if IS_SCARA @@ -1109,7 +1191,10 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { if (!position_is_reachable(destination)) return true; // Get the linear distance in XYZ - float cartesian_mm = xyz_float_t(diff).magnitude(); + #if HAS_ROTATIONAL_AXES + bool cartes_move = true; + #endif + float cartesian_mm = get_move_distance(diff OPTARG(HAS_ROTATIONAL_AXES, cartes_move)); // If the move is very short, check the E move distance TERN_(HAS_EXTRUDERS, if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e)); @@ -1118,7 +1203,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { if (UNEAR_ZERO(cartesian_mm)) return true; // Minimum number of seconds to move the given distance - const float seconds = cartesian_mm / scaled_fr_mm_s; + const float seconds = cartesian_mm / ( + #if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT) + cartes_move ? scaled_fr_mm_s : LINEAR_UNIT(scaled_fr_mm_s) + #else + scaled_fr_mm_s + #endif + ); // The number of segments-per-second times the duration // gives the number of segments @@ -1140,6 +1231,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { // Add hints to help optimize the move PlannerHints hints(cartesian_mm * inv_segments); + TERN_(HAS_ROTATIONAL_AXES, hints.cartesian_move = cartes_move); TERN_(FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters); /* @@ -1190,9 +1282,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { } // Get the linear distance in XYZ + #if HAS_ROTATIONAL_AXES + bool cartes_move = true; + #endif + float cartesian_mm = get_move_distance(diff OPTARG(HAS_ROTATIONAL_AXES, cartes_move)); + // If the move is very short, check the E move distance // No E move either? Game over. - float cartesian_mm = diff.magnitude(); TERN_(HAS_EXTRUDERS, if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = ABS(diff.e)); if (UNEAR_ZERO(cartesian_mm)) return; @@ -1207,6 +1303,7 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) { // Add hints to help optimize the move PlannerHints hints(cartesian_mm * inv_segments); + TERN_(HAS_ROTATIONAL_AXES, hints.cartesian_move = cartes_move); TERN_(FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters); //SERIAL_ECHOPGM("mm=", cartesian_mm); diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index f0e4094f0f34b..2acc54ebc2707 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -302,6 +302,8 @@ void report_current_position_projected(); #endif #endif +float get_move_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool &is_cartesian_move)); + void get_cartesian_from_steppers(); void set_current_from_steppers_for_axis(const AxisEnum axis); diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 46aff3248139a..ff5449d230ddc 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2130,8 +2130,8 @@ bool Planner::_populate_block( TERN_(LCD_SHOW_E_TOTAL, e_move_accumulator += steps_dist_mm.e); - #if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT) - bool cartesian_move = true; + #if HAS_ROTATIONAL_AXES + bool cartesian_move = hints.cartesian_move; #endif if (true NUM_AXIS_GANG( @@ -2152,71 +2152,34 @@ bool Planner::_populate_block( if (hints.millimeters) block->millimeters = hints.millimeters; else { - /** - * Distance for interpretation of feedrate in accordance with LinuxCNC (the successor of NIST - * RS274NGC interpreter - version 3) and its default CANON_XYZ feed reference mode. - * Assume that X, Y, Z are the primary linear axes and U, V, W are secondary linear axes and A, B, C are - * rotational axes. Then dX, dY, dZ are the displacements of the primary linear axes and dU, dV, dW are the displacements of linear axes and - * dA, dB, dC are the displacements of rotational axes. - * The time it takes to execute move command with feedrate F is t = D/F, where D is the total distance, calculated as follows: - * D^2 = dX^2 + dY^2 + dZ^2 - * if D^2 == 0 (none of XYZ move but any secondary linear axes move, whether other axes are moved or not): - * D^2 = dU^2 + dV^2 + dW^2 - * if D^2 == 0 (only rotational axes are moved): - * D^2 = dA^2 + dB^2 + dC^2 - */ - float distance_sqr = ( - #if ENABLED(ARTICULATED_ROBOT_ARM) - // For articulated robots, interpreting feedrate like LinuxCNC would require inverse kinematics. As a workaround, pretend that motors sit on n mutually orthogonal - // axes and assume that we could think of distance as magnitude of an n-vector in an n-dimensional Euclidian space. - NUM_AXIS_GANG( - sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z), - + sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k), - + sq(steps_dist_mm.u), + sq(steps_dist_mm.v), + sq(steps_dist_mm.w) - ) - #elif ENABLED(FOAMCUTTER_XYUV) - #if HAS_J_AXIS - // Special 5 axis kinematics. Return the largest distance move from either X/Y or I/J plane - _MAX(sq(steps_dist_mm.x) + sq(steps_dist_mm.y), sq(steps_dist_mm.i) + sq(steps_dist_mm.j)) - #else // Foamcutter with only two axes (XY) - sq(steps_dist_mm.x) + sq(steps_dist_mm.y) - #endif - #elif ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) - XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.z)) + const xyze_pos_t displacement = LOGICAL_AXIS_ARRAY( + steps_dist_mm.e, + #if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX) + steps_dist_mm.head.x, + steps_dist_mm.head.y, + steps_dist_mm.z, #elif CORE_IS_XZ - XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.head.z)) + steps_dist_mm.head.x, + steps_dist_mm.y, + steps_dist_mm.head.z, #elif CORE_IS_YZ - XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.head.z)) + steps_dist_mm.x, + steps_dist_mm.head.y, + steps_dist_mm.head.z, #else - XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z)) + steps_dist_mm.x, + steps_dist_mm.y, + steps_dist_mm.z, #endif + steps_dist_mm.i, + steps_dist_mm.j, + steps_dist_mm.k, + steps_dist_mm.u, + steps_dist_mm.v, + steps_dist_mm.w ); - #if SECONDARY_LINEAR_AXES && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM) - if (UNEAR_ZERO(distance_sqr)) { - // Move does not involve any primary linear axes (xyz) but might involve secondary linear axes - distance_sqr = (0.0f - SECONDARY_AXIS_GANG( - IF_DISABLED(AXIS4_ROTATES, + sq(steps_dist_mm.i)), - IF_DISABLED(AXIS5_ROTATES, + sq(steps_dist_mm.j)), - IF_DISABLED(AXIS6_ROTATES, + sq(steps_dist_mm.k)), - IF_DISABLED(AXIS7_ROTATES, + sq(steps_dist_mm.u)), - IF_DISABLED(AXIS8_ROTATES, + sq(steps_dist_mm.v)), - IF_DISABLED(AXIS9_ROTATES, + sq(steps_dist_mm.w)) - ) - ); - } - #endif - - #if HAS_ROTATIONAL_AXES && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM) - if (UNEAR_ZERO(distance_sqr)) { - // Move involves only rotational axes. Calculate angular distance in accordance with LinuxCNC - TERN_(INCH_MODE_SUPPORT, cartesian_move = false); - distance_sqr = ROTATIONAL_AXIS_GANG(sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k), + sq(steps_dist_mm.u), + sq(steps_dist_mm.v), + sq(steps_dist_mm.w)); - } - #endif - - block->millimeters = SQRT(distance_sqr); + block->millimeters = get_move_distance(displacement OPTARG(HAS_ROTATIONAL_AXES, cartesian_move)); } /** @@ -2354,12 +2317,13 @@ bool Planner::_populate_block( // Calculate inverse time for this move. No divide by zero due to previous checks. // Example: At 120mm/s a 60mm move involving XYZ axes takes 0.5s. So this will give 2.0. // Example 2: At 120°/s a 60° move involving only rotational axes takes 0.5s. So this will give 2.0. - float inverse_secs; - #if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT) - inverse_secs = inverse_millimeters * (cartesian_move ? fr_mm_s : LINEAR_UNIT(fr_mm_s)); - #else - inverse_secs = fr_mm_s * inverse_millimeters; - #endif + float inverse_secs = inverse_millimeters * ( + #if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT) + cartesian_move ? fr_mm_s : LINEAR_UNIT(fr_mm_s) + #else + fr_mm_s + #endif + ); // Get the number of non busy movements in queue (non busy means that they can be altered) const uint8_t moves_queued = nonbusy_movesplanned(); @@ -3157,9 +3121,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s PlannerHints ph = hints; if (!hints.millimeters) - ph.millimeters = (cart_dist_mm.x || cart_dist_mm.y) - ? xyz_pos_t(cart_dist_mm).magnitude() - : TERN0(HAS_Z_AXIS, ABS(cart_dist_mm.z)); + ph.millimeters = get_move_distance(xyze_pos_t(cart_dist_mm) OPTARG(HAS_ROTATIONAL_AXES, ph.cartesian_move)); #if DISABLED(FEEDRATE_SCALING) diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 7cc8bc08e4ebe..b63fabb84d025 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -377,6 +377,11 @@ struct PlannerHints { // would calculate if it knew the as-yet-unbuffered path #endif + #if HAS_ROTATIONAL_AXES + bool cartesian_move = true; // True if linear motion of the tool centerpoint relative to the workpiece occurs. + // False if no movement of the tool center point relative to the work piece occurs + // (i.e. the tool rotates around the tool centerpoint) + #endif PlannerHints(const_float_t mm=0.0f) : millimeters(mm) {} };