Michal
/
Prusa-Firmware-MMU
mirror of https://github.com/prusa3d/Prusa-Firmware-MMU
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix MMU3 fsensor handling

pull/360/head
Lapeno89 2026-03-31 16:46:02 +02:00
parent 48d4d920be
commit 2d11f5abfb
2 changed files with 174 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

227
src/logic/unload_to_finda.cpp
View File

@ -10,99 +10,158 @@
#include "../modules/pulley.h" #include "../modules/pulley.h"
#include "../modules/timebase.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 { namespace logic {
void UnloadToFinda::Reset(uint8_t maxTries) { void UnloadToFinda::Reset(uint8_t maxTries) {
this->maxTries = maxTries; this->maxTries = maxTries;
// check the inital state of FINDA and plan the moves // check the inital state of FINDA and plan the moves
if (!mf::finda.Pressed()) { if (!mf::finda.Pressed()) {
state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded state = OK; // FINDA is already off, we assume the filament is not there, i.e. already unloaded
} else { } else {
// FINDA is sensing the filament, plan moves to unload it // FINDA is sensing the filament, plan moves to unload it
state = EngagingIdler; state = EngagingIdler;
mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // basically prepare before the active slot - saves ~1s mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // prepare before the active slot - saves ~1s
started_ms = mt::timebase.Millis(); started_ms = 0; // start timeout later, when we actually wait for fsensor==OFF
ml::leds.ActiveSlotProcessing(); microPullTries = 0;
} microPullMovePlanned = false;
ml::leds.ActiveSlotProcessing();
}
} }
bool UnloadToFinda::Step() { bool UnloadToFinda::Step() {
switch (state) { switch (state) {
// start by engaging the idler into intermediate position // 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 // 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. // 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. // 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:
case EngagingIdler: if (!mi::idler.PartiallyDisengaged()) { // just waiting for Idler to get into the target intermediate position
if (!mi::idler.PartiallyDisengaged()) { // just waiting for Idler to get into the target intermediate position return false;
return false; }
} if (mfs::fsensor.Pressed()) { // still pressed, printer didn't free the filament yet
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 (mt::timebase.Elapsed(started_ms, 4000)) { if (started_ms == 0) started_ms = mt::timebase.Millis();
state = FailedFSensor; // fsensor didn't turn off within 4 seconds, something is seriously wrong // If timeout elapsed, trigger micro-pull strategy with idler fully engaged
} if (mt::timebase.Elapsed(started_ms, kFsensorWaitTimeout_ms)) {
return false; mpu::pulley.InitAxis();
} else { mi::idler.Engage(mg::globals.ActiveSlot());
// fsensor is OFF and Idler is partially engaged, engage the Idler fully and pull microPullMovePlanned = false; // plan in MicroPullTry
if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) { state = MicroPullTry;
state = UnloadingToFinda; }
mpu::pulley.InitAxis(); return false;
mi::idler.Engage(mg::globals.ActiveSlot()); } 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;
// slow move for the first few millimeters - help the printer relieve the filament while engaging the Idler fully case MicroPullTry:
mpu::pulley.PlanMove(-config::fsensorToNozzleAvoidGrindUnload, mg::globals.PulleySlowFeedrate_mm_s(), mg::globals.PulleySlowFeedrate_mm_s()); // Ensure the Idler is fully engaged before moving
} else { if (!mi::idler.Engaged()) {
state = FailedFINDA; return false;
}
}
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;
}
}
} }
return false; // Plan the 1mm micro-pull only once per attempt
case OK: if (!microPullMovePlanned) {
case FailedFINDA: mpu::pulley.PlanMove(-kMicroPull_mm,
case FailedFSensor: mg::globals.PulleySlowFeedrate_mm_s(),
default: mg::globals.PulleySlowFeedrate_mm_s());
return true; 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;
}
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;
}
}
}
return false;
case OK:
case FailedFINDA:
case FailedFSensor:
default:
return true;
}
} }
} // namespace logic } // namespace logic

57
src/logic/unload_to_finda.h
View File

@ -3,7 +3,6 @@
#include <stdint.h> #include <stdint.h>
namespace logic { namespace logic {
/// @brief Unload to FINDA "small" state machine /// @brief Unload to FINDA "small" state machine
/// ///
/// "small" state machines will serve as building blocks for high-level commands/operations /// "small" state machines will serve as building blocks for high-level commands/operations
@ -12,36 +11,42 @@ namespace logic {
/// - rotate some axis to some fixed direction /// - rotate some axis to some fixed direction
/// - load/unload to finda /// - load/unload to finda
struct UnloadToFinda { struct UnloadToFinda {
/// internal states of the state machine /// internal states of the state machine
enum : uint8_t { enum : uint8_t {
EngagingIdler, EngagingIdler,
UnloadingToFinda, MicroPullTry,
WaitingForFINDA, UnloadingToFinda,
OK, WaitingForFINDA,
FailedFINDA, OK,
FailedFSensor FailedFINDA,
}; FailedFSensor
inline constexpr UnloadToFinda() };
: state(OK)
, maxTries(3)
, unloadStart_mm(0)
, started_ms(0) {}
/// Restart the automaton inline constexpr UnloadToFinda()
/// @param maxTries maximum number of retried attempts before reporting a fail : state(OK)
void Reset(uint8_t maxTries); , maxTries(3)
, unloadStart_mm(0)
, started_ms(0)
, microPullTries(0)
, microPullMovePlanned(false) {}
/// @returns true if the state machine finished its job, false otherwise /// Restart the automaton
bool Step(); /// @param maxTries maximum number of retried attempts before reporting a fail
void Reset(uint8_t maxTries);
/// @returns internal state of the state machine /// @returns true if the state machine finished its job, false otherwise
inline uint8_t State() const { return state; } bool Step();
/// @returns internal state of the state machine
inline uint8_t State() const { return state; }
private: private:
uint8_t state; uint8_t state;
uint8_t maxTries; uint8_t maxTries;
int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (reps. long double) int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (resp. long double)
uint16_t started_ms; // timeout on fsensor turn off 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 } // namespace logic