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.
πŸ§‘β€πŸ’» Add get_move_distance for rotation/kinematics (MarlinFirmware#25370)
Browse files Browse the repository at this point in the history
DerAndere1 authored and EvilGremlin committed Apr 8, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
1 parent e995175 commit 87f1051
Showing 4 changed files with 139 additions and 73 deletions.
103 changes: 100 additions & 3 deletions Marlin/src/module/motion.cpp
Original file line number Diff line number Diff line change
@@ -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);
2 changes: 2 additions & 0 deletions Marlin/src/module/motion.h
Original file line number Diff line number Diff line change
@@ -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);

102 changes: 32 additions & 70 deletions Marlin/src/module/planner.cpp
Original file line number Diff line number Diff line change
@@ -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)

5 changes: 5 additions & 0 deletions Marlin/src/module/planner.h
Original file line number Diff line number Diff line change
@@ -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) {}
};

0 comments on commit 87f1051

Please sign in to comment.