Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
misc. cleanup
Browse files Browse the repository at this point in the history
thinkyhead committed Feb 16, 2023
1 parent b723565 commit 183aab0
Showing 2 changed files with 21 additions and 13 deletions.
21 changes: 14 additions & 7 deletions Marlin/src/module/motion.cpp
Original file line number Diff line number Diff line change
@@ -1068,21 +1068,27 @@ float get_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool &is_c
}
else {
#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)
#if HAS_J_AXIS
// Special 5 axis kinematics. Return the largest distance move from either X/Y or I/J plane
const float distance_sqr = _MAX(sq(diff.x) + sq(diff.y), sq(diff.i) + sq(diff.j));
#else // Foamcutter with only two axes (XY)
const float distance_sqr = sq(diff.x) + sq(diff.y);
#endif

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:
@@ -1110,7 +1116,7 @@ float get_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool &is_c
#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 = (0.0f
distance_sqr = (
SECONDARY_AXIS_GANG(
IF_DISABLED(AXIS4_ROTATES, + sq(diff.i)),
IF_DISABLED(AXIS5_ROTATES, + sq(diff.j)),
@@ -1130,6 +1136,7 @@ float get_distance(const xyze_pos_t &diff OPTARG(HAS_ROTATIONAL_AXES, bool &is_c
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);
13 changes: 7 additions & 6 deletions Marlin/src/module/planner.cpp
Original file line number Diff line number Diff line change
@@ -2317,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();

0 comments on commit 183aab0

Please sign in to comment.