Skip to content

Fix MMU3 fsensor handling #360

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Lapeno89 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fix MMU3 fsensor handling #360

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
227 changes: 143 additions & 84 deletions src/logic/unload_to_finda.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,99 +10,158 @@
#include "../modules/pulley.h"
#include "../modules/timebase.h"

namespace {
// Local parameters; if you prefer, move them to config::
constexpr uint16_t kFsensorWaitTimeout_ms = 4000; // how long we wait for fsensor==OFF before micro-pull
constexpr uint8_t kMaxMicroPullTries = 2; // number of 1mm micro-pull attempts
constexpr unit::U_mm kMicroPull_mm = 1.0_mm; // distance of each micro-pull
}

namespace logic {

void UnloadToFinda::Reset(uint8_t maxTries) {
this->maxTries = maxTries;
// check the inital state of FINDA and plan the moves
if (!mf::finda.Pressed()) {
state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded
} else {
// FINDA is sensing the filament, plan moves to unload it
state = EngagingIdler;
mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // basically prepare before the active slot - saves ~1s
started_ms = mt::timebase.Millis();
ml::leds.ActiveSlotProcessing();
}
this->maxTries = maxTries;
// check the inital state of FINDA and plan the moves
if (!mf::finda.Pressed()) {
state = OK; // FINDA is already off, we assume the filament is not there, i.e. already unloaded
} else {
// FINDA is sensing the filament, plan moves to unload it
state = EngagingIdler;
mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // prepare before the active slot - saves ~1s
started_ms = 0; // start timeout later, when we actually wait for fsensor==OFF
microPullTries = 0;
microPullMovePlanned = false;
ml::leds.ActiveSlotProcessing();
}
}

bool UnloadToFinda::Step() {
switch (state) {
// start by engaging the idler into intermediate position
// Then, wait for !fsensor.Pressed: that's to speed-up the pull process - unload operation will be started during the purging moves
// and as soon as the fsensor turns off, the MMU engages the idler fully and starts pulling.
// It will not wait for the extruder to finish the relieve move.
// However, such an approach breaks running the MMU on a non-reworked MK4/C1, which hasn't been officially supported, but possible (with some level of uncertainity).
case EngagingIdler:
if (!mi::idler.PartiallyDisengaged()) { // just waiting for Idler to get into the target intermediate position
return false;
}
if (mfs::fsensor.Pressed()) { // still pressed, printer didn't free the filament yet
if (mt::timebase.Elapsed(started_ms, 4000)) {
state = FailedFSensor; // fsensor didn't turn off within 4 seconds, something is seriously wrong
}
return false;
} else {
// fsensor is OFF and Idler is partially engaged, engage the Idler fully and pull
if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
state = UnloadingToFinda;
mpu::pulley.InitAxis();
mi::idler.Engage(mg::globals.ActiveSlot());
switch (state) {
// start by engaging the idler into intermediate position
// Then, wait for !fsensor.Pressed: to speed-up the pull process - unload operation will be started during the purging moves
// and as soon as the fsensor turns off, the MMU engages the idler fully and starts pulling.
// It will not wait for the extruder to finish the relieve move.
case EngagingIdler:
if (!mi::idler.PartiallyDisengaged()) { // just waiting for Idler to get into the target intermediate position
return false;
}
if (mfs::fsensor.Pressed()) { // still pressed, printer didn't free the filament yet
// Start the timeout NOW (not in Reset), only when we actually wait for fsensor==OFF
if (started_ms == 0) started_ms = mt::timebase.Millis();
// If timeout elapsed, trigger micro-pull strategy with idler fully engaged
if (mt::timebase.Elapsed(started_ms, kFsensorWaitTimeout_ms)) {
mpu::pulley.InitAxis();
mi::idler.Engage(mg::globals.ActiveSlot());
microPullMovePlanned = false; // plan in MicroPullTry
state = MicroPullTry;
}
return false;
} else {
// fsensor is OFF and Idler is partially engaged, engage the Idler fully and pull
if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
state = UnloadingToFinda;
mpu::pulley.InitAxis();
mi::idler.Engage(mg::globals.ActiveSlot());
// slow move for the first few millimeters - help the printer relieve the filament while engaging the Idler fully
mpu::pulley.PlanMove(-config::fsensorToNozzleAvoidGrindUnload,
mg::globals.PulleySlowFeedrate_mm_s(),
mg::globals.PulleySlowFeedrate_mm_s());
} else {
state = FailedFINDA;
}
}
return false;

case MicroPullTry:
// Ensure the Idler is fully engaged before moving
if (!mi::idler.Engaged()) {
return false;
}
// Plan the 1mm micro-pull only once per attempt
if (!microPullMovePlanned) {
mpu::pulley.PlanMove(-kMicroPull_mm,
mg::globals.PulleySlowFeedrate_mm_s(),
mg::globals.PulleySlowFeedrate_mm_s());
microPullMovePlanned = true;
return false;
}
// Wait for the micro move to finish
if (!mm::motion.QueueEmpty()) {
return false;
}
// After the micro-pull, check FSensor
if (!mfs::fsensor.Pressed()) {
// FSensor finally OFF -> proceed with normal unload
if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
state = UnloadingToFinda;
// The UnloadingToFinda state will plan the long unload move
} else {
state = FailedFINDA;
}
microPullMovePlanned = false;
return false;
}
// Still ON -> try again up to kMaxMicroPullTries
if (++microPullTries < kMaxMicroPullTries) {
microPullMovePlanned = false; // schedule another micro-pull
return false;
} else {
// attempts exhausted -> fail safely
state = FailedFSensor;
microPullMovePlanned = false;
return true;
}

// slow move for the first few millimeters - help the printer relieve the filament while engaging the Idler fully
mpu::pulley.PlanMove(-config::fsensorToNozzleAvoidGrindUnload, mg::globals.PulleySlowFeedrate_mm_s(), mg::globals.PulleySlowFeedrate_mm_s());
} else {
state = FailedFINDA;
}
}
return false;
case UnloadingToFinda:
if (mi::idler.Engaged()) {
state = WaitingForFINDA;
mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
unloadStart_mm = mpu::pulley.CurrentPosition_mm();
// We can always plan the unload move for the maximum allowed bowden length,
// it should be even more reliable than doing just the specified bowden length:
// - if the filament is slipping for some reason, planning a longer move will not stop in the middle of the bowden tube
// - a faster unload (shorter than the specified bowden length) will be interrupted by FINDA turning off
// - if FINDA is misaligned or faulty, the only issue will be, that the filament will be thrown behind the pulley
// which could have happened with the previous implementation as well, because default bowden length was set to 42cm
mpu::pulley.PlanMove(-config::maximumBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, mg::globals.PulleyUnloadFeedrate_mm_s());
}
return false;
case WaitingForFINDA: {
int32_t currentPulley_mm = mpu::pulley.CurrentPosition_mm();
if ((abs(unloadStart_mm - currentPulley_mm) > mm::truncatedUnit(mg::globals.FSensorUnloadCheck_mm())) && mfs::fsensor.Pressed()) {
// fsensor didn't trigger within the first fsensorUnloadCheckDistance mm -> stop pulling, something failed, report an error
// This scenario should not be tried again - repeating it may cause more damage to filament + potentially more collateral damage
state = FailedFSensor;
mm::motion.AbortPlannedMoves(); // stop rotating the pulley
ml::leds.ActiveSlotDoneEmpty();
} else if (!mf::finda.Pressed()) {
// detected end of filament
state = OK;
mm::motion.AbortPlannedMoves(); // stop rotating the pulley
ml::leds.ActiveSlotDoneEmpty();
} else if (/*tmc2130_read_gstat() &&*/ mm::motion.QueueEmpty()) {
// we reached the end of move queue, but the FINDA didn't switch off
// two possible causes - grinded filament or malfunctioning FINDA
if (--maxTries) {
// Ideally, the Idler shall rehome and then try again.
// That would auto-resolve errors caused by slipped or misaligned Idler
mi::idler.InvalidateHoming();
Reset(maxTries);
} else {
state = FailedFINDA;
}
}
case UnloadingToFinda:
if (mi::idler.Engaged()) {
state = WaitingForFINDA;
mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
unloadStart_mm = mpu::pulley.CurrentPosition_mm();
// We can always plan the unload move for the maximum allowed bowden length,
// it should be even more reliable than doing just the specified bowden length:
// - if the filament is slipping for some reason, planning a longer move will not stop in the middle of the bowden tube
// - a faster unload (shorter than the specified bowden length) will be interrupted by FINDA turning off
// - if FINDA is misaligned or faulty, the only issue will be, that the filament will be thrown behind the pulley
// which could have happened with the previous implementation as well, because default bowden length was set to 42cm
mpu::pulley.PlanMove(-config::maximumBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU,
mg::globals.PulleyUnloadFeedrate_mm_s());
}
return false;
case OK:
case FailedFINDA:
case FailedFSensor:
default:
return true;
return false;

case WaitingForFINDA: {
int32_t currentPulley_mm = mpu::pulley.CurrentPosition_mm();
if ((abs(unloadStart_mm - currentPulley_mm) > mm::truncatedUnit(mg::globals.FSensorUnloadCheck_mm())) && mfs::fsensor.Pressed()) {
// fsensor didn't trigger within the first fsensorUnloadCheckDistance mm -> stop pulling, something failed, report an error
// This scenario should not be tried again - repeating it may cause more damage to filament + potentially more collateral damage
state = FailedFSensor;
mm::motion.AbortPlannedMoves(); // stop rotating the pulley
ml::leds.ActiveSlotDoneEmpty();
} else if (!mf::finda.Pressed()) {
// detected end of filament
state = OK;
mm::motion.AbortPlannedMoves(); // stop rotating the pulley
ml::leds.ActiveSlotDoneEmpty();
} else if (/*tmc2130_read_gstat() &&*/ mm::motion.QueueEmpty()) {
// we reached the end of move queue, but the FINDA didn't switch off
// two possible causes - grinded filament or malfunctioning FINDA
if (--maxTries) {
// Ideally, the Idler shall rehome and then try again.
// That would auto-resolve errors caused by slipped or misaligned Idler
mi::idler.InvalidateHoming();
Reset(maxTries);
} else {
state = FailedFINDA;
}
}
}
return false;

case OK:
case FailedFINDA:
case FailedFSensor:
default:
return true;
}
}

} // namespace logic
57 changes: 31 additions & 26 deletions src/logic/unload_to_finda.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
#include <stdint.h>

namespace logic {

/// @brief Unload to FINDA "small" state machine
///
/// "small" state machines will serve as building blocks for high-level commands/operations
Expand All @@ -12,36 +11,42 @@ namespace logic {
/// - rotate some axis to some fixed direction
/// - load/unload to finda
struct UnloadToFinda {
/// internal states of the state machine
enum : uint8_t {
EngagingIdler,
UnloadingToFinda,
WaitingForFINDA,
OK,
FailedFINDA,
FailedFSensor
};
inline constexpr UnloadToFinda()
: state(OK)
, maxTries(3)
, unloadStart_mm(0)
, started_ms(0) {}
/// internal states of the state machine
enum : uint8_t {
EngagingIdler,
MicroPullTry,
UnloadingToFinda,
WaitingForFINDA,
OK,
FailedFINDA,
FailedFSensor
};

inline constexpr UnloadToFinda()
: state(OK)
, maxTries(3)
, unloadStart_mm(0)
, started_ms(0)
, microPullTries(0)
, microPullMovePlanned(false) {}

/// Restart the automaton
/// @param maxTries maximum number of retried attempts before reporting a fail
void Reset(uint8_t maxTries);
/// Restart the automaton
/// @param maxTries maximum number of retried attempts before reporting a fail
void Reset(uint8_t maxTries);

/// @returns true if the state machine finished its job, false otherwise
bool Step();
/// @returns true if the state machine finished its job, false otherwise
bool Step();

/// @returns internal state of the state machine
inline uint8_t State() const { return state; }
/// @returns internal state of the state machine
inline uint8_t State() const { return state; }

private:
uint8_t state;
uint8_t maxTries;
int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (reps. long double)
uint16_t started_ms; // timeout on fsensor turn off
uint8_t state;
uint8_t maxTries;
int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (resp. long double)
uint16_t started_ms; // timeout window while actually waiting for fsensor to turn off
uint8_t microPullTries; // how many micro-pull attempts have been executed
bool microPullMovePlanned; // whether the 1mm micro-pull move has been queued
};

} // namespace logic
Loading