diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h index edc18e5..d535cbf 100644 --- a/src/logic/progress_codes.h +++ b/src/logic/progress_codes.h @@ -50,5 +50,7 @@ enum class ProgressCode : uint_fast8_t { HWTestDisplay, // P35 ErrHwTestFailed, // P36 + UnloadingFilamentSlowly, // P37 + Empty = 0xff // dummy empty state }; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 951003c..b8bbecf 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -133,4 +133,20 @@ bool UnloadFilament::StepInner() { return false; } +ProgressCode UnloadFilament::State() const { + switch (state) { + case ProgressCode::UnloadingToFinda: + switch (unl.State()) { + case UnloadToFinda::EngagingIdler: + return ProgressCode::EngagingIdler; + case UnloadToFinda::UnloadingFromFSensor: + return ProgressCode::UnloadingFilamentSlowly; + // the rest should be reported as UnloadingFilament + } + [[fallthrough]]; + default: + return state; + } +} + } // namespace logic diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h index c22bedd..4bc774d 100644 --- a/src/logic/unload_filament.h +++ b/src/logic/unload_filament.h @@ -21,6 +21,8 @@ public: /// @returns true if the state machine finished its job, false otherwise bool StepInner() override; + ProgressCode State() const override; + private: constexpr static const uint8_t maxRetries = 1; diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp index f24754b..d7e3247 100644 --- a/src/logic/unload_to_finda.cpp +++ b/src/logic/unload_to_finda.cpp @@ -20,37 +20,46 @@ void UnloadToFinda::Reset(uint8_t maxTries) { // FINDA is sensing the filament, plan moves to unload it state = EngagingIdler; mi::idler.Engage(mg::globals.ActiveSlot()); + if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) { + state = UnloadingFromFSensor; + mpu::pulley.InitAxis(); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); + } else { + state = FailedFINDA; + } } } bool UnloadToFinda::Step() { switch (state) { case EngagingIdler: - if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) { - state = UnloadingToFinda; - mpu::pulley.InitAxis(); - ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); - } else { - state = FailedFINDA; - } - return false; - case UnloadingToFinda: if (mi::idler.Engaged()) { - state = WaitingForFINDA; + state = UnloadingFromFSensor; mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector); unloadStart_mm = mpu::pulley.CurrentPosition_mm(); + // plan both moves to keep the unload smooth + mpu::pulley.PlanMove(-mg::globals.FSensorUnloadCheck_mm(), mg::globals.PulleySlowFeedrate_mm_s()); mpu::pulley.PlanMove(-config::defaultBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, mg::globals.PulleyUnloadFeedrate_mm_s()); } return false; + case UnloadingFromFSensor: + if ((abs(unloadStart_mm - mpu::pulley.CurrentPosition_mm()) > mm::truncatedUnit(mg::globals.FSensorUnloadCheck_mm()))) { + // passed the slow unload distance, check fsensor + if (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.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off); + } else { + // fsensor turned correctly off, seamlessly transfer to fast unloading -> waiting for FINDA + // the move has already been planned when idler engaged + state = WaitingForFINDA; + } + } + 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.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off); - } else if (!mf::finda.Pressed()) { + if (!mf::finda.Pressed()) { // detected end of filament state = OK; mm::motion.AbortPlannedMoves(); // stop rotating the pulley diff --git a/src/logic/unload_to_finda.h b/src/logic/unload_to_finda.h index b84e516..0d9d8fd 100644 --- a/src/logic/unload_to_finda.h +++ b/src/logic/unload_to_finda.h @@ -15,7 +15,7 @@ struct UnloadToFinda { /// internal states of the state machine enum { EngagingIdler, - UnloadingToFinda, + UnloadingFromFSensor, WaitingForFINDA, OK, FailedFINDA, diff --git a/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp b/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp index 31304d9..c9e066c 100644 --- a/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp +++ b/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp @@ -57,7 +57,7 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") { CHECK(mm::axes[mm::Pulley].enabled == true); // now pulling the filament until finda triggers - REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA); + REQUIRE(ff.State() == logic::UnloadToFinda::UnloadingFromFSensor); REQUIRE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 10, 1000), 1100)); REQUIRE(ff.State() == logic::UnloadToFinda::OK); @@ -209,7 +209,7 @@ TEST_CASE("unload_to_finda::unload_repeated", "[unload_to_finda]") { main_loop(); ff.Step(); - REQUIRE(ff.State() == logic::UnloadToFinda::UnloadingToFinda); + REQUIRE(ff.State() == logic::UnloadToFinda::UnloadingFromFSensor); REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector); // make arbitrary amount of steps