sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

🧑‍💻 Add get_move_distance for rotation/kinematics (#25370)

Browse Source
This commit is contained in:
DerAndere
2023-02-21 19:26:10 +01:00
committed by GitHub
parent 122d4a89f6
commit babd3b0037
4 changed files with 139 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

102
Marlin/src/module/planner.cpp
View File

@@ -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)