Scott Lahteine
@ -1300,7 +1300,7 @@ void Planner::recalculate() {
|
||||
*/
|
||||
void Planner::check_axes_activity() {
|
||||
|
||||
#if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_I, DISABLE_J, DISABLE_K, DISABLE_E)
|
||||
#if ANY(DISABLE_X, DISABLE_Y, DISABLE_Z, DISABLE_I, DISABLE_J, DISABLE_K, DISABLE_U, DISABLE_V, DISABLE_W, DISABLE_E)
|
||||
xyze_bool_t axis_active = { false };
|
||||
#endif
|
||||
|
||||
@ -1350,7 +1350,10 @@ void Planner::check_axes_activity() {
|
||||
if (TERN0(DISABLE_Z, bnext->steps.z)) axis_active.z = true,
|
||||
if (TERN0(DISABLE_I, bnext->steps.i)) axis_active.i = true,
|
||||
if (TERN0(DISABLE_J, bnext->steps.j)) axis_active.j = true,
|
||||
if (TERN0(DISABLE_K, bnext->steps.k)) axis_active.k = true
|
||||
if (TERN0(DISABLE_K, bnext->steps.k)) axis_active.k = true,
|
||||
if (TERN0(DISABLE_U, bnext->steps.u)) axis_active.u = true,
|
||||
if (TERN0(DISABLE_V, bnext->steps.v)) axis_active.v = true,
|
||||
if (TERN0(DISABLE_W, bnext->steps.w)) axis_active.w = true
|
||||
);
|
||||
}
|
||||
#endif
|
||||
@ -1385,7 +1388,10 @@ void Planner::check_axes_activity() {
|
||||
if (TERN0(DISABLE_Z, !axis_active.z)) stepper.disable_axis(Z_AXIS),
|
||||
if (TERN0(DISABLE_I, !axis_active.i)) stepper.disable_axis(I_AXIS),
|
||||
if (TERN0(DISABLE_J, !axis_active.j)) stepper.disable_axis(J_AXIS),
|
||||
if (TERN0(DISABLE_K, !axis_active.k)) stepper.disable_axis(K_AXIS)
|
||||
if (TERN0(DISABLE_K, !axis_active.k)) stepper.disable_axis(K_AXIS),
|
||||
if (TERN0(DISABLE_U, !axis_active.u)) stepper.disable_axis(U_AXIS),
|
||||
if (TERN0(DISABLE_V, !axis_active.v)) stepper.disable_axis(V_AXIS),
|
||||
if (TERN0(DISABLE_W, !axis_active.w)) stepper.disable_axis(W_AXIS)
|
||||
);
|
||||
|
||||
//
|
||||
@ -1453,7 +1459,7 @@ void Planner::check_axes_activity() {
|
||||
float high = 0.0f;
|
||||
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
|
||||
const block_t * const block = &block_buffer[b];
|
||||
if (LINEAR_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k)) {
|
||||
if (NUM_AXIS_GANG(block->steps.x, || block->steps.y, || block->steps.z, || block->steps.i, || block->steps.j, || block->steps.k, || block->steps.u, || block->steps.v, || block->steps.w)) {
|
||||
const float se = (float)block->steps.e / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
|
||||
NOLESS(high, se);
|
||||
}
|
||||
@ -1843,15 +1849,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
dc = target.c - position.c,
|
||||
di = target.i - position.i,
|
||||
dj = target.j - position.j,
|
||||
dk = target.k - position.k
|
||||
dk = target.k - position.k,
|
||||
du = target.u - position.u,
|
||||
dv = target.v - position.v,
|
||||
dw = target.w - position.w
|
||||
);
|
||||
|
||||
/* <-- add a slash to enable
|
||||
SERIAL_ECHOLNPGM(
|
||||
" _populate_block FR:", fr_mm_s,
|
||||
" A:", target.a, " (", da, " steps)"
|
||||
" B:", target.b, " (", db, " steps)"
|
||||
" C:", target.c, " (", dc, " steps)"
|
||||
#if HAS_Y_AXIS
|
||||
" B:", target.b, " (", db, " steps)"
|
||||
#endif
|
||||
#if HAS_Z_AXIS
|
||||
" C:", target.c, " (", dc, " steps)"
|
||||
#endif
|
||||
#if HAS_I_AXIS
|
||||
" " STR_I ":", target.i, " (", di, " steps)"
|
||||
#endif
|
||||
@ -1861,6 +1874,14 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
#if HAS_K_AXIS
|
||||
" " STR_K ":", target.k, " (", dk, " steps)"
|
||||
#endif
|
||||
#if HAS_U_AXIS
|
||||
" " STR_U ":", target.u, " (", du, " steps)"
|
||||
#endif
|
||||
#if HAS_V_AXIS
|
||||
" " STR_V ":", target.v, " (", dv, " steps)"
|
||||
#endif
|
||||
#if HAS_W_AXIS
|
||||
" " STR_W ":", target.w, " (", dw, " steps)"
|
||||
#if HAS_EXTRUDERS
|
||||
" E:", target.e, " (", de, " steps)"
|
||||
#endif
|
||||
@ -1925,15 +1946,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
|
||||
if (CORESIGN(db - dc) < 0) SBI(dm, C_AXIS); // Motor C direction
|
||||
#endif
|
||||
#if HAS_I_AXIS
|
||||
if (di < 0) SBI(dm, I_AXIS);
|
||||
#endif
|
||||
#if HAS_J_AXIS
|
||||
if (dj < 0) SBI(dm, J_AXIS);
|
||||
#endif
|
||||
#if HAS_K_AXIS
|
||||
if (dk < 0) SBI(dm, K_AXIS);
|
||||
#endif
|
||||
#elif ENABLED(MARKFORGED_XY)
|
||||
if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
|
||||
if (db < 0) SBI(dm, B_AXIS); // Motor B direction
|
||||
@ -1941,16 +1953,22 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
if (da < 0) SBI(dm, A_AXIS); // Motor A direction
|
||||
if (db + da < 0) SBI(dm, B_AXIS); // Motor B direction
|
||||
#else
|
||||
LINEAR_AXIS_CODE(
|
||||
XYZ_CODE(
|
||||
if (da < 0) SBI(dm, X_AXIS),
|
||||
if (db < 0) SBI(dm, Y_AXIS),
|
||||
if (dc < 0) SBI(dm, Z_AXIS),
|
||||
if (di < 0) SBI(dm, I_AXIS),
|
||||
if (dj < 0) SBI(dm, J_AXIS),
|
||||
if (dk < 0) SBI(dm, K_AXIS)
|
||||
if (dc < 0) SBI(dm, Z_AXIS)
|
||||
);
|
||||
#endif
|
||||
|
||||
SECONDARY_AXIS_CODE(
|
||||
if (di < 0) SBI(dm, I_AXIS),
|
||||
if (dj < 0) SBI(dm, J_AXIS),
|
||||
if (dk < 0) SBI(dm, K_AXIS),
|
||||
if (du < 0) SBI(dm, U_AXIS),
|
||||
if (dv < 0) SBI(dm, V_AXIS),
|
||||
if (dw < 0) SBI(dm, W_AXIS)
|
||||
);
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
if (de < 0) SBI(dm, E_AXIS);
|
||||
const float esteps_float = de * e_factor[extruder];
|
||||
@ -1974,22 +1992,24 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
|
||||
// Number of steps for each axis
|
||||
// See https://www.corexy.com/theory.html
|
||||
#if CORE_IS_XY
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da + db), ABS(da - db), ABS(dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#elif CORE_IS_XZ
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da + dc), ABS(db), ABS(da - dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#elif CORE_IS_YZ
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da), ABS(db + dc), ABS(db - dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#elif ENABLED(MARKFORGED_XY)
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da + db), ABS(db), ABS(dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#elif ENABLED(MARKFORGED_YX)
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da), ABS(db + da), ABS(dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#elif IS_SCARA
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da), ABS(db), ABS(dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#else
|
||||
// default non-h-bot planning
|
||||
block->steps.set(LINEAR_AXIS_LIST(ABS(da), ABS(db), ABS(dc), ABS(di), ABS(dj), ABS(dk)));
|
||||
#endif
|
||||
block->steps.set(NUM_AXIS_LIST(
|
||||
#if CORE_IS_XY
|
||||
ABS(da + db), ABS(da - db), ABS(dc)
|
||||
#elif CORE_IS_XZ
|
||||
ABS(da + dc), ABS(db), ABS(da - dc)
|
||||
#elif CORE_IS_YZ
|
||||
ABS(da), ABS(db + dc), ABS(db - dc)
|
||||
#elif ENABLED(MARKFORGED_XY)
|
||||
ABS(da + db), ABS(db), ABS(dc)
|
||||
#elif ENABLED(MARKFORGED_YX)
|
||||
ABS(da), ABS(db + da), ABS(dc)
|
||||
#elif IS_SCARA
|
||||
ABS(da), ABS(db), ABS(dc)
|
||||
#else // default non-h-bot planning
|
||||
ABS(da), ABS(db), ABS(dc)
|
||||
#endif
|
||||
, ABS(di), ABS(dj), ABS(dk), ABS(du), ABS(dv), ABS(dw)
|
||||
));
|
||||
|
||||
/**
|
||||
* This part of the code calculates the total length of the movement.
|
||||
@ -2027,9 +2047,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
steps_dist_mm.b = (db + dc) * mm_per_step[B_AXIS];
|
||||
steps_dist_mm.c = CORESIGN(db - dc) * mm_per_step[C_AXIS];
|
||||
#endif
|
||||
TERN_(HAS_I_AXIS, steps_dist_mm.i = di * mm_per_step[I_AXIS]);
|
||||
TERN_(HAS_J_AXIS, steps_dist_mm.j = dj * mm_per_step[J_AXIS]);
|
||||
TERN_(HAS_K_AXIS, steps_dist_mm.k = dk * mm_per_step[K_AXIS]);
|
||||
#elif ENABLED(MARKFORGED_XY)
|
||||
steps_dist_mm.a = (da - db) * mm_per_step[A_AXIS];
|
||||
steps_dist_mm.b = db * mm_per_step[B_AXIS];
|
||||
@ -2037,27 +2054,40 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
steps_dist_mm.a = da * mm_per_step[A_AXIS];
|
||||
steps_dist_mm.b = (db - da) * mm_per_step[B_AXIS];
|
||||
#else
|
||||
LINEAR_AXIS_CODE(
|
||||
XYZ_CODE(
|
||||
steps_dist_mm.a = da * mm_per_step[A_AXIS],
|
||||
steps_dist_mm.b = db * mm_per_step[B_AXIS],
|
||||
steps_dist_mm.c = dc * mm_per_step[C_AXIS],
|
||||
steps_dist_mm.i = di * mm_per_step[I_AXIS],
|
||||
steps_dist_mm.j = dj * mm_per_step[J_AXIS],
|
||||
steps_dist_mm.k = dk * mm_per_step[K_AXIS]
|
||||
steps_dist_mm.c = dc * mm_per_step[C_AXIS]
|
||||
);
|
||||
#endif
|
||||
|
||||
SECONDARY_AXIS_CODE(
|
||||
steps_dist_mm.i = di * mm_per_step[I_AXIS],
|
||||
steps_dist_mm.j = dj * mm_per_step[J_AXIS],
|
||||
steps_dist_mm.k = dk * mm_per_step[K_AXIS],
|
||||
steps_dist_mm.u = du * mm_per_step[U_AXIS],
|
||||
steps_dist_mm.v = dv * mm_per_step[V_AXIS],
|
||||
steps_dist_mm.w = dw * mm_per_step[W_AXIS]
|
||||
);
|
||||
|
||||
TERN_(HAS_EXTRUDERS, steps_dist_mm.e = esteps_float * mm_per_step[E_AXIS_N(extruder)]);
|
||||
|
||||
TERN_(LCD_SHOW_E_TOTAL, e_move_accumulator += steps_dist_mm.e);
|
||||
|
||||
if (true LINEAR_AXIS_GANG(
|
||||
#if BOTH(HAS_ROTATIONAL_AXES, INCH_MODE_SUPPORT)
|
||||
bool cartesian_move = true;
|
||||
#endif
|
||||
|
||||
if (true NUM_AXIS_GANG(
|
||||
&& block->steps.a < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.b < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.c < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.i < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.j < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.k < MIN_STEPS_PER_SEGMENT
|
||||
&& block->steps.k < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.u < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.v < MIN_STEPS_PER_SEGMENT,
|
||||
&& block->steps.w < MIN_STEPS_PER_SEGMENT
|
||||
)
|
||||
) {
|
||||
block->millimeters = TERN0(HAS_EXTRUDERS, ABS(steps_dist_mm.e));
|
||||
@ -2066,36 +2096,71 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
if (millimeters)
|
||||
block->millimeters = millimeters;
|
||||
else {
|
||||
block->millimeters = SQRT(
|
||||
#if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
|
||||
LINEAR_AXIS_GANG(
|
||||
sq(steps_dist_mm.head.x), + sq(steps_dist_mm.head.y), + sq(steps_dist_mm.z),
|
||||
+ sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k)
|
||||
)
|
||||
#elif CORE_IS_XZ
|
||||
LINEAR_AXIS_GANG(
|
||||
sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.head.z),
|
||||
+ sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k)
|
||||
)
|
||||
#elif CORE_IS_YZ
|
||||
LINEAR_AXIS_GANG(
|
||||
sq(steps_dist_mm.x) + sq(steps_dist_mm.head.y) + sq(steps_dist_mm.head.z)
|
||||
+ sq(steps_dist_mm.i), + sq(steps_dist_mm.j), + sq(steps_dist_mm.k)
|
||||
)
|
||||
/**
|
||||
* 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)
|
||||
// Return the largest distance move from either X/Y or I/J plane
|
||||
#if HAS_J_AXIS
|
||||
_MAX(sq(steps_dist_mm.x) + sq(steps_dist_mm.y), sq(steps_dist_mm.i) + sq(steps_dist_mm.j))
|
||||
#else
|
||||
// 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))
|
||||
#elif CORE_IS_XZ
|
||||
XYZ_GANG(sq(steps_dist_mm.head.x), + sq(steps_dist_mm.y), + sq(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))
|
||||
#else
|
||||
LINEAR_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)
|
||||
)
|
||||
XYZ_GANG(sq(steps_dist_mm.x), + sq(steps_dist_mm.y), + sq(steps_dist_mm.z))
|
||||
#endif
|
||||
);
|
||||
|
||||
#if SECONDARY_LINEAR_AXES >= 1 && NONE(FOAMCUTTER_XYUV, ARTICULATED_ROBOT_ARM)
|
||||
if (NEAR_ZERO(distance_sqr)) {
|
||||
// Move does not involve any primary linear axes (xyz) but might involve secondary linear axes
|
||||
distance_sqr = (0.0
|
||||
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 (NEAR_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);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -2112,8 +2177,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
|
||||
TERN_(HAS_EXTRUDERS, block->steps.e = esteps);
|
||||
|
||||
block->step_event_count = _MAX(LOGICAL_AXIS_LIST(
|
||||
esteps, block->steps.a, block->steps.b, block->steps.c, block->steps.i, block->steps.j, block->steps.k
|
||||
block->step_event_count = _MAX(LOGICAL_AXIS_LIST(esteps,
|
||||
block->steps.a, block->steps.b, block->steps.c,
|
||||
block->steps.i, block->steps.j, block->steps.k,
|
||||
block->steps.u, block->steps.v, block->steps.w
|
||||
));
|
||||
|
||||
// Bail if this is a zero-length block
|
||||
@ -2135,13 +2202,16 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
E_TERN_(block->extruder = extruder);
|
||||
|
||||
#if ENABLED(AUTO_POWER_CONTROL)
|
||||
if (LINEAR_AXIS_GANG(
|
||||
if (NUM_AXIS_GANG(
|
||||
block->steps.x,
|
||||
|| block->steps.y,
|
||||
|| block->steps.z,
|
||||
|| block->steps.i,
|
||||
|| block->steps.j,
|
||||
|| block->steps.k
|
||||
|| block->steps.k,
|
||||
|| block->steps.u,
|
||||
|| block->steps.v,
|
||||
|| block->steps.w
|
||||
)) powerManager.power_on();
|
||||
#endif
|
||||
|
||||
@ -2167,19 +2237,27 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
}
|
||||
if (block->steps.x) stepper.enable_axis(X_AXIS);
|
||||
#else
|
||||
LINEAR_AXIS_CODE(
|
||||
NUM_AXIS_CODE(
|
||||
if (block->steps.x) stepper.enable_axis(X_AXIS),
|
||||
if (block->steps.y) stepper.enable_axis(Y_AXIS),
|
||||
if (TERN(Z_LATE_ENABLE, 0, block->steps.z)) stepper.enable_axis(Z_AXIS),
|
||||
if (block->steps.i) stepper.enable_axis(I_AXIS),
|
||||
if (block->steps.j) stepper.enable_axis(J_AXIS),
|
||||
if (block->steps.k) stepper.enable_axis(K_AXIS)
|
||||
if (block->steps.k) stepper.enable_axis(K_AXIS),
|
||||
if (block->steps.u) stepper.enable_axis(U_AXIS),
|
||||
if (block->steps.v) stepper.enable_axis(V_AXIS),
|
||||
if (block->steps.w) stepper.enable_axis(W_AXIS)
|
||||
);
|
||||
#endif
|
||||
#if ANY(CORE_IS_XY, MARKFORGED_XY, MARKFORGED_YX)
|
||||
TERN_(HAS_I_AXIS, if (block->steps.i) stepper.enable_axis(I_AXIS));
|
||||
TERN_(HAS_J_AXIS, if (block->steps.j) stepper.enable_axis(J_AXIS));
|
||||
TERN_(HAS_K_AXIS, if (block->steps.k) stepper.enable_axis(K_AXIS));
|
||||
SECONDARY_AXIS_CODE(
|
||||
if (block->steps.i) stepper.enable_axis(I_AXIS),
|
||||
if (block->steps.j) stepper.enable_axis(J_AXIS),
|
||||
if (block->steps.k) stepper.enable_axis(K_AXIS),
|
||||
if (block->steps.u) stepper.enable_axis(U_AXIS),
|
||||
if (block->steps.v) stepper.enable_axis(V_AXIS),
|
||||
if (block->steps.w) stepper.enable_axis(W_AXIS)
|
||||
);
|
||||
#endif
|
||||
|
||||
// Enable extruder(s)
|
||||
@ -2226,8 +2304,14 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
const float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides
|
||||
|
||||
// Calculate inverse time for this move. No divide by zero due to previous checks.
|
||||
// Example: At 120mm/s a 60mm move takes 0.5s. So this will give 2.0.
|
||||
float inverse_secs = fr_mm_s * inverse_millimeters;
|
||||
// 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
|
||||
|
||||
// Get the number of non busy movements in queue (non busy means that they can be altered)
|
||||
const uint8_t moves_queued = nonbusy_movesplanned();
|
||||
@ -2273,13 +2357,13 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
filwidth.advance_e(steps_dist_mm.e);
|
||||
#endif
|
||||
|
||||
// Calculate and limit speed in mm/sec
|
||||
// Calculate and limit speed in mm/sec (linear) or degrees/sec (rotational)
|
||||
|
||||
xyze_float_t current_speed;
|
||||
float speed_factor = 1.0f; // factor <1 decreases speed
|
||||
|
||||
// Linear axes first with less logic
|
||||
LOOP_LINEAR_AXES(i) {
|
||||
LOOP_NUM_AXES(i) {
|
||||
current_speed[i] = steps_dist_mm[i] * inverse_secs;
|
||||
const feedRate_t cs = ABS(current_speed[i]),
|
||||
max_fr = settings.max_feedrate_mm_s[i];
|
||||
@ -2367,9 +2451,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
// Compute and limit the acceleration rate for the trapezoid generator.
|
||||
const float steps_per_mm = block->step_event_count * inverse_millimeters;
|
||||
uint32_t accel;
|
||||
if (LINEAR_AXIS_GANG(
|
||||
if (NUM_AXIS_GANG(
|
||||
!block->steps.a, && !block->steps.b, && !block->steps.c,
|
||||
&& !block->steps.i, && !block->steps.j, && !block->steps.k)
|
||||
&& !block->steps.i, && !block->steps.j, && !block->steps.k,
|
||||
&& !block->steps.u, && !block->steps.v, && !block->steps.w)
|
||||
) { // Is this a retract / recover move?
|
||||
accel = CEIL(settings.retract_acceleration * steps_per_mm); // Convert to: acceleration steps/sec^2
|
||||
TERN_(LIN_ADVANCE, block->use_advance_lead = false); // No linear advance for simple retract/recover
|
||||
@ -2442,7 +2527,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
LIMIT_ACCEL_LONG(C_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(I_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(J_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(K_AXIS, 0)
|
||||
LIMIT_ACCEL_LONG(K_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(U_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(V_AXIS, 0),
|
||||
LIMIT_ACCEL_LONG(W_AXIS, 0)
|
||||
);
|
||||
}
|
||||
else {
|
||||
@ -2453,7 +2541,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
LIMIT_ACCEL_FLOAT(C_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(I_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(J_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(K_AXIS, 0)
|
||||
LIMIT_ACCEL_FLOAT(K_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(U_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(V_AXIS, 0),
|
||||
LIMIT_ACCEL_FLOAT(W_AXIS, 0)
|
||||
);
|
||||
}
|
||||
}
|
||||
@ -2518,7 +2609,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
#if HAS_DIST_MM_ARG
|
||||
cart_dist_mm
|
||||
#else
|
||||
LOGICAL_AXIS_ARRAY(steps_dist_mm.e, steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z, steps_dist_mm.i, steps_dist_mm.j, steps_dist_mm.k)
|
||||
LOGICAL_AXIS_ARRAY(steps_dist_mm.e, steps_dist_mm.x, steps_dist_mm.y, steps_dist_mm.z, steps_dist_mm.i, steps_dist_mm.j, steps_dist_mm.k, steps_dist_mm.u, steps_dist_mm.v, steps_dist_mm.w)
|
||||
#endif
|
||||
;
|
||||
|
||||
@ -2544,7 +2635,10 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
+ (-prev_unit_vec.z * unit_vec.z),
|
||||
+ (-prev_unit_vec.i * unit_vec.i),
|
||||
+ (-prev_unit_vec.j * unit_vec.j),
|
||||
+ (-prev_unit_vec.k * unit_vec.k)
|
||||
+ (-prev_unit_vec.k * unit_vec.k),
|
||||
+ (-prev_unit_vec.u * unit_vec.u),
|
||||
+ (-prev_unit_vec.v * unit_vec.v),
|
||||
+ (-prev_unit_vec.w * unit_vec.w)
|
||||
);
|
||||
|
||||
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
|
||||
@ -2691,7 +2785,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
const float extra_xyjerk = TERN0(HAS_EXTRUDERS, de <= 0) ? TRAVEL_EXTRA_XYJERK : 0;
|
||||
|
||||
uint8_t limited = 0;
|
||||
TERN(HAS_LINEAR_E_JERK, LOOP_LINEAR_AXES, LOOP_LOGICAL_AXES)(i) {
|
||||
TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(i) {
|
||||
const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed for this axis
|
||||
maxj = (max_jerk[i] + (i == X_AXIS || i == Y_AXIS ? extra_xyjerk : 0.0f)); // mj : The max jerk setting for this axis
|
||||
if (jerk > maxj) { // cs > mj : New current speed too fast?
|
||||
@ -2729,7 +2823,7 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
||||
vmax_junction = previous_nominal_speed;
|
||||
|
||||
// Now limit the jerk in all axes.
|
||||
TERN(HAS_LINEAR_E_JERK, LOOP_LINEAR_AXES, LOOP_LOGICAL_AXES)(axis) {
|
||||
TERN(HAS_LINEAR_E_JERK, LOOP_NUM_AXES, LOOP_LOGICAL_AXES)(axis) {
|
||||
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
|
||||
float v_exit = previous_speed[axis] * smaller_speed_factor,
|
||||
v_entry = current_speed[axis];
|
||||
@ -2831,7 +2925,7 @@ void Planner::buffer_sync_block(TERN_(LASER_SYNCHRONOUS_M106_M107, uint8_t sync_
|
||||
|
||||
block->position = position;
|
||||
#if ENABLED(BACKLASH_COMPENSATION)
|
||||
LOOP_LINEAR_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis);
|
||||
LOOP_NUM_AXES(axis) block->position[axis] += backlash.get_applied_steps((AxisEnum)axis);
|
||||
#endif
|
||||
|
||||
#if BOTH(HAS_FAN, LASER_SYNCHRONOUS_M106_M107)
|
||||
@ -2893,7 +2987,10 @@ bool Planner::buffer_segment(const abce_pos_t &abce
|
||||
int32_t(LROUND(abce.c * settings.axis_steps_per_mm[C_AXIS])),
|
||||
int32_t(LROUND(abce.i * settings.axis_steps_per_mm[I_AXIS])),
|
||||
int32_t(LROUND(abce.j * settings.axis_steps_per_mm[J_AXIS])),
|
||||
int32_t(LROUND(abce.k * settings.axis_steps_per_mm[K_AXIS]))
|
||||
int32_t(LROUND(abce.k * settings.axis_steps_per_mm[K_AXIS])),
|
||||
int32_t(LROUND(abce.u * settings.axis_steps_per_mm[U_AXIS])),
|
||||
int32_t(LROUND(abce.v * settings.axis_steps_per_mm[V_AXIS])),
|
||||
int32_t(LROUND(abce.w * settings.axis_steps_per_mm[W_AXIS]))
|
||||
)
|
||||
};
|
||||
|
||||
@ -2945,6 +3042,21 @@ bool Planner::buffer_segment(const abce_pos_t &abce
|
||||
SERIAL_ECHOPGM(" (", position.k, "->", target.k);
|
||||
SERIAL_CHAR(')');
|
||||
#endif
|
||||
#if HAS_U_AXIS
|
||||
SERIAL_ECHOPGM_P(SP_U_LBL, abce.u);
|
||||
SERIAL_ECHOPGM(" (", position.u, "->", target.u);
|
||||
SERIAL_CHAR(')');
|
||||
#endif
|
||||
#if HAS_V_AXIS
|
||||
SERIAL_ECHOPGM_P(SP_V_LBL, abce.v);
|
||||
SERIAL_ECHOPGM(" (", position.v, "->", target.v);
|
||||
SERIAL_CHAR(')');
|
||||
#endif
|
||||
#if HAS_W_AXIS
|
||||
SERIAL_ECHOPGM_P(SP_W_LBL, abce.w);
|
||||
SERIAL_ECHOPGM(" (", position.w, "->", target.w);
|
||||
SERIAL_CHAR(')');
|
||||
#endif
|
||||
#if HAS_EXTRUDERS
|
||||
SERIAL_ECHOPGM_P(SP_E_LBL, abce.e);
|
||||
SERIAL_ECHOLNPGM(" (", position.e, "->", target.e, ")");
|
||||
@ -2987,12 +3099,14 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s, cons
|
||||
const xyze_pos_t cart_dist_mm = LOGICAL_AXIS_ARRAY(
|
||||
cart.e - position_cart.e,
|
||||
cart.x - position_cart.x, cart.y - position_cart.y, cart.z - position_cart.z,
|
||||
cart.i - position_cart.i, cart.j - position_cart.j, cart.j - position_cart.k
|
||||
cart.i - position_cart.i, cart.j - position_cart.j, cart.k - position_cart.k,
|
||||
cart.u - position_cart.u, cart.v - position_cart.v, cart.w - position_cart.w
|
||||
);
|
||||
#else
|
||||
const xyz_pos_t cart_dist_mm = LINEAR_AXIS_ARRAY(
|
||||
const xyz_pos_t cart_dist_mm = NUM_AXIS_ARRAY(
|
||||
cart.x - position_cart.x, cart.y - position_cart.y, cart.z - position_cart.z,
|
||||
cart.i - position_cart.i, cart.j - position_cart.j, cart.j - position_cart.k
|
||||
cart.i - position_cart.i, cart.j - position_cart.j, cart.k - position_cart.k,
|
||||
cart.u - position_cart.u, cart.v - position_cart.v, cart.w - position_cart.w
|
||||
);
|
||||
#endif
|
||||
|
||||
@ -3097,7 +3211,10 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) {
|
||||
LROUND(abce.c * settings.axis_steps_per_mm[C_AXIS]),
|
||||
LROUND(abce.i * settings.axis_steps_per_mm[I_AXIS]),
|
||||
LROUND(abce.j * settings.axis_steps_per_mm[J_AXIS]),
|
||||
LROUND(abce.k * settings.axis_steps_per_mm[K_AXIS])
|
||||
LROUND(abce.k * settings.axis_steps_per_mm[K_AXIS]),
|
||||
LROUND(abce.u * settings.axis_steps_per_mm[U_AXIS]),
|
||||
LROUND(abce.v * settings.axis_steps_per_mm[V_AXIS]),
|
||||
LROUND(abce.w * settings.axis_steps_per_mm[W_AXIS])
|
||||
)
|
||||
);
|
||||
|
||||
@ -3109,7 +3226,7 @@ void Planner::set_machine_position_mm(const abce_pos_t &abce) {
|
||||
else {
|
||||
#if ENABLED(BACKLASH_COMPENSATION)
|
||||
abce_long_t stepper_pos = position;
|
||||
LOOP_LINEAR_AXES(axis) stepper_pos[axis] += backlash.get_applied_steps((AxisEnum)axis);
|
||||
LOOP_NUM_AXES(axis) stepper_pos[axis] += backlash.get_applied_steps((AxisEnum)axis);
|
||||
stepper.set_position(stepper_pos);
|
||||
#else
|
||||
stepper.set_position(position);
|
||||
|
||||
Reference in New Issue
Block a user