mirror of
https://github.com/MarlinFirmware/Marlin.git
synced 2025-12-29 02:40:41 -07:00
parent
a54daeeb39
commit
a614352fcf
3 changed files with 21 additions and 25 deletions
|
|
@ -42,7 +42,7 @@ ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepp
|
|||
int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
|
||||
FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
|
||||
|
||||
bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use.
|
||||
bool FTMotion::stepperCmdBuffHasData = false; // The stepper buffer has items and is in use.
|
||||
|
||||
XYZEval<millis_t> FTMotion::axis_move_end_ti = { 0 };
|
||||
AxisBits FTMotion::axis_move_dir;
|
||||
|
|
@ -64,9 +64,9 @@ bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done b
|
|||
// ... if applicable, and is ready to be converted to step commands.
|
||||
|
||||
// Trapezoid data variables.
|
||||
xyze_pos_t FTMotion::startPosn, // (mm) Start position of block
|
||||
FTMotion::endPosn_prevBlock = { 0.0f }; // (mm) End position of previous block
|
||||
xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block
|
||||
xyze_pos_t FTMotion::startPos, // (mm) Start position of block
|
||||
FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block
|
||||
xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block
|
||||
float FTMotion::accel_P, // Acceleration prime of block. [mm/sec/sec]
|
||||
FTMotion::decel_P, // Deceleration prime of block. [mm/sec/sec]
|
||||
FTMotion::F_P, // Feedrate prime of block. [mm/sec]
|
||||
|
|
@ -135,7 +135,7 @@ void FTMotion::loop() {
|
|||
* 4. Signal ready for new block.
|
||||
*/
|
||||
if (stepper.abort_current_block) {
|
||||
if (sts_stepperBusy) return; // Wait until motion buffers are emptied
|
||||
if (stepperCmdBuffHasData) return; // Wait until motion buffers are emptied
|
||||
discard_planner_block_protected();
|
||||
reset();
|
||||
stepper.abort_current_block = false; // Abort finished.
|
||||
|
|
@ -149,13 +149,13 @@ void FTMotion::loop() {
|
|||
continue;
|
||||
}
|
||||
loadBlockData(stepper.current_block);
|
||||
blockProcRdy = true;
|
||||
|
||||
// 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();
|
||||
|
||||
blockProcRdy = true;
|
||||
// Some kinematics track axis motion in HX, HY, HZ
|
||||
#if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX)
|
||||
stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx;
|
||||
|
|
@ -170,18 +170,14 @@ void FTMotion::loop() {
|
|||
|
||||
if (blockProcRdy) {
|
||||
|
||||
if (!batchRdy) makeVector(); // Caution: Do not consolidate checks on blockProcRdy/batchRdy, as they are written by makeVector().
|
||||
// When makeVector is finished: either blockProcRdy has been set false (because the block is
|
||||
// done being processed) or batchRdy is set true, or both.
|
||||
if (!batchRdy) makeVector(); // may clear blockProcRdy
|
||||
|
||||
// Check if the block has been completely converted:
|
||||
if (!blockProcRdy) {
|
||||
discard_planner_block_protected();
|
||||
|
||||
// Check if the block needs to be runout:
|
||||
if (!batchRdy && !planner.has_blocks_queued()) {
|
||||
runoutBlock();
|
||||
makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop.
|
||||
makeVector(); // Additional call to guarantee batchRdy is set this loop.
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -220,7 +216,7 @@ void FTMotion::loop() {
|
|||
}
|
||||
|
||||
// Report busy status to planner.
|
||||
busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp);
|
||||
busy = (stepperCmdBuffHasData || blockProcRdy || batchRdy || batchRdyForInterp);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -386,7 +382,7 @@ void FTMotion::reset() {
|
|||
|
||||
blockProcRdy = batchRdy = batchRdyForInterp = false;
|
||||
|
||||
endPosn_prevBlock.reset();
|
||||
endPos_prevBlock.reset();
|
||||
|
||||
makeVector_idx = 0;
|
||||
makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE));
|
||||
|
|
@ -424,13 +420,13 @@ void FTMotion::discard_planner_block_protected() {
|
|||
/**
|
||||
* Set up a pseudo block to allow motion to settle and buffers to empty.
|
||||
* Called when the planner has one block left. The buffers will be filled
|
||||
* with the last commanded position by setting the startPosn block variable to
|
||||
* with the last commanded position by setting the startPos block variable to
|
||||
* the last position of the previous block and all ratios to zero such that no
|
||||
* axes' positions are incremented.
|
||||
*/
|
||||
void FTMotion::runoutBlock() {
|
||||
|
||||
startPosn = endPosn_prevBlock;
|
||||
startPos = endPos_prevBlock;
|
||||
ratio.reset();
|
||||
|
||||
const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - makeVector_batchIdx;
|
||||
|
|
@ -466,7 +462,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
|
|||
const float totalLength = current_block->millimeters,
|
||||
oneOverLength = 1.0f / totalLength;
|
||||
|
||||
startPosn = endPosn_prevBlock;
|
||||
startPos = endPos_prevBlock;
|
||||
|
||||
const xyze_pos_t& moveDist = current_block->dist_mm;
|
||||
|
||||
|
|
@ -551,7 +547,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
|
|||
// Accel + Coasting + Decel datapoints
|
||||
max_intervals = N1 + N2 + N3;
|
||||
|
||||
endPosn_prevBlock += moveDist;
|
||||
endPos_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)));
|
||||
|
|
@ -592,7 +588,7 @@ void FTMotion::makeVector() {
|
|||
TERN_(HAS_EXTRUDERS, accel_k = decel_P); // (mm/s^2) Acceleration K factor from Decel phase
|
||||
}
|
||||
|
||||
#define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPosn.q + ratio.q * dist;
|
||||
#define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPos.q + ratio.q * dist;
|
||||
LOGICAL_AXIS_MAP_LC(_SET_TRAJ);
|
||||
|
||||
#if HAS_EXTRUDERS
|
||||
|
|
|
|||
|
|
@ -113,7 +113,7 @@ class FTMotion {
|
|||
static int32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer.
|
||||
stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer.
|
||||
|
||||
static bool sts_stepperBusy; // The stepper buffer has items and is in use.
|
||||
static bool stepperCmdBuffHasData; // The stepper buffer has items and is in use.
|
||||
|
||||
static XYZEval<millis_t> axis_move_end_ti;
|
||||
static AxisBits axis_move_dir;
|
||||
|
|
@ -145,9 +145,9 @@ class FTMotion {
|
|||
static bool batchRdy, batchRdyForInterp;
|
||||
|
||||
// Trapezoid data variables.
|
||||
static xyze_pos_t startPosn, // (mm) Start position of block
|
||||
endPosn_prevBlock; // (mm) End position of previous block
|
||||
static xyze_float_t ratio; // (ratio) Axis move ratio of block
|
||||
static xyze_pos_t startPos, // (mm) Start position of block
|
||||
endPos_prevBlock; // (mm) End position of previous block
|
||||
static xyze_float_t ratio; // (ratio) Axis move ratio of block
|
||||
static float accel_P, decel_P,
|
||||
F_P,
|
||||
f_s,
|
||||
|
|
|
|||
|
|
@ -3565,8 +3565,8 @@ void Stepper::report_positions() {
|
|||
void Stepper::ftMotion_stepper() {
|
||||
|
||||
// Check if the buffer is empty.
|
||||
ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx);
|
||||
if (!ftMotion.sts_stepperBusy) return;
|
||||
ftMotion.stepperCmdBuffHasData = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx);
|
||||
if (!ftMotion.stepperCmdBuffHasData) return;
|
||||
|
||||
// "Pop" one command from current motion buffer
|
||||
const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue