Skip to content

Commit

Permalink
🐛 FT Motion time-based endstop (#27349)
Browse files Browse the repository at this point in the history 
Co-authored-by: ulendoalex <[email protected]>
  • Loading branch information
@thinkyhead @ulendoalex
thinkyhead and ulendoalex authored Aug 24, 2024
1 parent e2d8b2f commit 934ac24
Show file tree
Hide file tree
Showing 5 changed files with 80 additions and 59 deletions.
50 changes: 30 additions & 20 deletions Marlin/src/module/endstops.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,9 @@
#include "temperature.h"
#include "../lcd/marlinui.h"

#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif

#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
#include HAL_PATH(.., endstop_interrupts.h)
Expand All @@ -54,6 +55,9 @@
#include "probe.h"
#endif

#define DEBUG_OUT ALL(USE_SENSORLESS, DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"

Endstops endstops;

// private:
Expand Down Expand Up @@ -830,9 +834,13 @@ void Endstops::update() {

// Signal, after validation, if an endstop limit is pressed or not

#define AXIS_IS_MOVING(A) TERN(FT_MOTION, ftMotion, stepper).axis_is_moving(_AXIS(A))
#define AXIS_DIR_REV(A) !TERN(FT_MOTION, ftMotion, stepper).motor_direction(A)

#if HAS_X_AXIS
if (stepper.axis_is_moving(X_AXIS)) {
if (!stepper.motor_direction(X_AXIS_HEAD)) {
if (AXIS_IS_MOVING(X)) {
const AxisEnum x_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? X_AXIS : X_AXIS_HEAD;
if (AXIS_DIR_REV(x_head)) {
#if HAS_X_MIN_STATE
PROCESS_ENDSTOP_X(MIN);
#if CORE_DIAG(XY, Y, MIN)
Expand Down Expand Up @@ -864,8 +872,9 @@ void Endstops::update() {
#endif // HAS_X_AXIS

#if HAS_Y_AXIS
if (stepper.axis_is_moving(Y_AXIS)) {
if (!stepper.motor_direction(Y_AXIS_HEAD)) {
if (AXIS_IS_MOVING(Y)) {
const AxisEnum y_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Y_AXIS : Y_AXIS_HEAD;
if (AXIS_DIR_REV(y_head)) {
#if HAS_Y_MIN_STATE
PROCESS_ENDSTOP_Y(MIN);
#if CORE_DIAG(XY, X, MIN)
Expand Down Expand Up @@ -897,8 +906,9 @@ void Endstops::update() {
#endif // HAS_Y_AXIS

#if HAS_Z_AXIS
if (stepper.axis_is_moving(Z_AXIS)) {
if (!stepper.motor_direction(Z_AXIS_HEAD)) {
if (AXIS_IS_MOVING(Z)) {
const AxisEnum z_head = TERN0(FT_MOTION, ftMotion.cfg.active) ? Z_AXIS : Z_AXIS_HEAD;
if (AXIS_DIR_REV(z_head)) {
// Z- : Gantry down, bed up
#if HAS_Z_MIN_STATE
// If the Z_MIN_PIN is being used for the probe there's no
Expand Down Expand Up @@ -944,8 +954,8 @@ void Endstops::update() {
#endif // HAS_Z_AXIS

#if HAS_I_AXIS && HAS_I_STATE
if (stepper.axis_is_moving(I_AXIS)) {
if (!stepper.motor_direction(I_AXIS_HEAD)) {
if (AXIS_IS_MOVING(I)) {
if (AXIS_DIR_REV(I_AXIS_HEAD)) {
#if HAS_I_MIN_STATE
PROCESS_ENDSTOP(I, MIN);
#endif
Expand All @@ -959,8 +969,8 @@ void Endstops::update() {
#endif // HAS_I_AXIS

#if HAS_J_AXIS && HAS_J_STATE
if (stepper.axis_is_moving(J_AXIS)) {
if (!stepper.motor_direction(J_AXIS_HEAD)) {
if (AXIS_IS_MOVING(J)) {
if (AXIS_DIR_REV(J_AXIS_HEAD)) {
#if HAS_J_MIN_STATE
PROCESS_ENDSTOP(J, MIN);
#endif
Expand All @@ -974,8 +984,8 @@ void Endstops::update() {
#endif // HAS_J_AXIS

#if HAS_K_AXIS && HAS_K_STATE
if (stepper.axis_is_moving(K_AXIS)) {
if (!stepper.motor_direction(K_AXIS_HEAD)) {
if (AXIS_IS_MOVING(K)) {
if (AXIS_DIR_REV(K_AXIS_HEAD)) {
#if HAS_K_MIN_STATE
PROCESS_ENDSTOP(K, MIN);
#endif
Expand All @@ -989,8 +999,8 @@ void Endstops::update() {
#endif // HAS_K_AXIS

#if HAS_U_AXIS && HAS_U_STATE
if (stepper.axis_is_moving(U_AXIS)) {
if (!stepper.motor_direction(U_AXIS_HEAD)) {
if (AXIS_IS_MOVING(U)) {
if (AXIS_DIR_REV(U_AXIS_HEAD)) {
#if HAS_U_MIN_STATE
PROCESS_ENDSTOP(U, MIN);
#endif
Expand All @@ -1004,8 +1014,8 @@ void Endstops::update() {
#endif // HAS_U_AXIS

#if HAS_V_AXIS && HAS_V_STATE
if (stepper.axis_is_moving(V_AXIS)) {
if (!stepper.motor_direction(V_AXIS_HEAD)) {
if (AXIS_IS_MOVING(V)) {
if (AXIS_DIR_REV(V_AXIS_HEAD)) {
#if HAS_V_MIN_STATE
PROCESS_ENDSTOP(V, MIN);
#endif
Expand All @@ -1019,8 +1029,8 @@ void Endstops::update() {
#endif // HAS_V_AXIS

#if HAS_W_AXIS && HAS_W_STATE
if (stepper.axis_is_moving(W_AXIS)) {
if (!stepper.motor_direction(W_AXIS_HEAD)) {
if (AXIS_IS_MOVING(W)) {
if (AXIS_DIR_REV(W_AXIS_HEAD)) {
#if HAS_W_MIN_STATE
PROCESS_ENDSTOP(W, MIN);
#endif
Expand Down
40 changes: 30 additions & 10 deletions Marlin/src/module/ft_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman

bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use.

XYZEval<millis_t> FTMotion::axis_move_end_ti = { 0 };
AxisBits FTMotion::axis_move_dir;

// Private variables.

Expand Down Expand Up @@ -112,8 +114,6 @@ constexpr uint32_t BATCH_SIDX_IN_WINDOW = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);

// Public functions.

static bool markBlockStart = false;

// Controller main, to be invoked from non-isr task.
void FTMotion::loop() {

Expand Down Expand Up @@ -141,7 +141,6 @@ void FTMotion::loop() {
continue;
}
loadBlockData(stepper.current_block);
markBlockStart = true;
blockProcRdy = true;
// Some kinematics track axis motion in HX, HY, HZ
#if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX)
Expand Down Expand Up @@ -390,6 +389,8 @@ void FTMotion::reset() {
#endif

TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);

axis_move_end_ti.reset();
}

// Private functions.
Expand Down Expand Up @@ -429,7 +430,11 @@ void FTMotion::runoutBlock() {

const int32_t n_to_settle_and_fill_batch = n_to_settle_shaper + n_to_fill_batch_after_settling;

max_intervals = (PROP_BATCHES) * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;
const int32_t N_needed_to_propagate_to_stepper = PROP_BATCHES;

const int32_t n_to_use = N_needed_to_propagate_to_stepper * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch;

max_intervals = n_to_use;

blockProcRdy = true;
}
Expand Down Expand Up @@ -549,6 +554,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {

endPosn_prevBlock += moveDist;

// Watch endstops until the move ends
const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS)));

#define __SET_MOVE_END(A,V) do{ if (V) { axis_move_end_ti.A = move_end_ti; axis_move_dir.A = (V > 0); } }while(0);
#define _SET_MOVE_END(A) __SET_MOVE_END(A, moveDist[_AXIS(A)])
#if CORE_IS_XY
__SET_MOVE_END(X, moveDist.x + moveDist.y);
__SET_MOVE_END(Y, moveDist.x - moveDist.y);
#else
_SET_MOVE_END(X);
_SET_MOVE_END(Y);
#endif
TERN_(HAS_Z_AXIS, _SET_MOVE_END(Z));
SECONDARY_AXIS_MAP(_SET_MOVE_END);

// If the endstop is already pressed, endstop interrupts won't invoke
// endstop_triggered and the move will grind. So check here for a
// triggered endstop, which shortly marks the block for discard.
endstops.update();

}

// Generate data points of the trajectory.
Expand All @@ -565,6 +590,7 @@ void FTMotion::makeVector() {
else if (makeVector_idx < (N1 + N2)) {
// Coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block
//accel_k = 0.0f;
}
else {
// Deceleration phase
Expand Down Expand Up @@ -717,12 +743,6 @@ void FTMotion::convertToSteps(const uint32_t idx) {
// Init all step/dir bits to 0 (defaulting to reverse/negative motion)
cmd = 0;

// Mark the start of a new block
if (markBlockStart) {
cmd = _BV(FT_BIT_START);
markBlockStart = false;
}

// Accumulate the errors for all axes
err_P += delta;

Expand Down
11 changes: 11 additions & 0 deletions Marlin/src/module/ft_motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@

#include "../inc/MarlinConfigPre.h" // Access the top level configurations.
#include "../module/planner.h" // Access block type from planner.
#include "../module/stepper.h" // For stepper motion and direction

#include "ft_types.h"

Expand Down Expand Up @@ -110,6 +111,9 @@ class FTMotion {

static bool sts_stepperBusy; // The stepper buffer has items and is in use.

static XYZEval<millis_t> axis_move_end_ti;
static AxisBits axis_move_dir;

// Public methods
static void init();
static void loop(); // Controller main, to be invoked from non-isr task.
Expand All @@ -121,6 +125,13 @@ class FTMotion {

static void reset(); // Reset all states of the fixed time conversion to defaults.

FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) {
return cfg.active ? PENDING(millis(), axis_move_end_ti[axis]) : stepper.axis_is_moving(axis);
}
FORCE_INLINE static bool motor_direction(const AxisEnum axis) {
return cfg.active ? axis_move_dir[axis] : stepper.last_direction_bits[axis];
}

private:

static xyze_trajectory_t traj;
Expand Down
1 change: 0 additions & 1 deletion Marlin/src/module/ft_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;

// TODO: Convert ft_command_t to a struct with bitfields instead of using a primitive type
enum {
FT_BIT_START,
LIST_N(DOUBLE(LOGICAL_AXES),
FT_BIT_DIR_E, FT_BIT_STEP_E,
FT_BIT_DIR_X, FT_BIT_STEP_X, FT_BIT_DIR_Y, FT_BIT_STEP_Y, FT_BIT_DIR_Z, FT_BIT_STEP_Z,
Expand Down
37 changes: 9 additions & 28 deletions Marlin/src/module/stepper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1531,6 +1531,7 @@ void Stepper::isr() {
uint8_t max_loops = 10;

#if ENABLED(FT_MOTION)
static uint32_t ftMotion_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks.
const bool using_ftMotion = ftMotion.cfg.active;
#else
constexpr bool using_ftMotion = false;
Expand All @@ -1550,21 +1551,15 @@ void Stepper::isr() {
if (!nextMainISR) { // Main ISR is ready to fire during this iteration?
nextMainISR = FTM_MIN_TICKS; // Set to minimum interval (a limit on the top speed)
ftMotion_stepper(); // Run FTM Stepping
}

#if ENABLED(BABYSTEPPING)
if (nextBabystepISR == 0) { // Avoid ANY stepping too soon after baby-stepping
nextBabystepISR = babystepping_isr();
NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step
// Define 2.5 msec task for auxilliary functions.
if (!ftMotion_nextAuxISR) {
TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
ftMotion_nextAuxISR = (STEPPER_TIMER_RATE) / 400;
}
if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping
NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping
#endif

interval = nextMainISR; // Interval is either some old nextMainISR or FTM_MIN_TICKS
TERN_(BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping?

nextMainISR = 0; // For FT Motion fire again ASAP
}
interval = _MIN(nextMainISR, ftMotion_nextAuxISR);
nextMainISR -= interval;
ftMotion_nextAuxISR -= interval;
}

#endif
Expand Down Expand Up @@ -3537,15 +3532,6 @@ void Stepper::report_positions() {
#define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS)
#define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS)

/**
* Set bits in axis_did_move for any axes moving in this block,
* clearing the bits at the start of each new segment.
*/
if (TEST(command, FT_BIT_START)) axis_did_move.reset();

#define _FTM_AXIS_DID_MOVE(AXIS) axis_did_move.bset(_AXIS(AXIS), _FTM_STEP(AXIS));
LOGICAL_AXIS_MAP(_FTM_AXIS_DID_MOVE);

/**
* Update direction bits for steppers that were stepped by this command.
* HX, HY, HZ direction bits were set for Core kinematics
Expand Down Expand Up @@ -3607,11 +3593,6 @@ void Stepper::report_positions() {
#define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false);
LOGICAL_AXIS_MAP(_FTM_STEP_STOP);

// Check endstops on every step using axis_did_move as set by every step
// TODO: Update endstop states less frequently to save processing.
// NOTE: endstops.poll is still called at 1KHz by Temperature ISR.
IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, if ((bool)axis_did_move) endstops.update());

} // Stepper::ftMotion_stepper

#endif // FT_MOTION
Expand Down

0 comments on commit 934ac24

Please sign in to comment.