3DPrinting/MarlinFirmware
0
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2025-03-13 01:40:09 +00:00
Code Issues 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
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 139 additions and 73 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

103
Marlin/src/module/motion.cpp
View file

@ -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
Marlin/src/module/motion.h
View file

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

5
Marlin/src/module/planner.h
View file

@ -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) {}
};