diff --git a/src/config/config.h b/src/config/config.h index 1e0b139..7bad99f 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -83,15 +83,17 @@ static constexpr U_mm couplerToBowden = 3.5_mm; /// 3.5_mm /// FINDA Coupler scr // just another piece of PLA (probably having more resistance in the tubes) // and we are at least 40mm off! It looks like this really depends on the exact position // We'll probably need to check for stallguard while pushing the filament to avoid ginding the filament -static constexpr U_mm defaultBowdenLength = 427.0_mm; /// ~427.0_mm /// Default Bowden length. @TODO Should be stored in EEPROM. 392 a 784 -static constexpr U_mm minimumBowdenLength = 341.0_mm; /// ~341.0_mm /// Minimum bowden length. @TODO Should be stored in EEPROM. -static constexpr U_mm maximumBowdenLength = 792.0_mm; /// ~792.0_mm /// Maximum bowden length. @TODO Should be stored in EEPROM. +static constexpr U_mm defaultBowdenLength = 427.0_mm; ///< ~427.0_mm - Default Bowden length. @TODO Should be stored in EEPROM. 392 a 784 +static constexpr U_mm minimumBowdenLength = 341.0_mm; ///< ~341.0_mm - Minimum bowden length. @TODO Should be stored in EEPROM. +static constexpr U_mm maximumBowdenLength = 792.0_mm; ///< ~792.0_mm - Maximum bowden length. @TODO Should be stored in EEPROM. static constexpr U_mm feedToFinda = cuttingEdgeToFindaMidpoint + filamentMinLoadedToMMU; static constexpr U_mm maximumFeedToFinda = feedToFinda + 20.0_mm; ///< allow for some safety margin to load to FINDA -static constexpr U_mm pulleyHelperMove = 10.0_mm; /// Helper move for Load/Unload error states - when the MMU should slowly move the filament a bit +static constexpr U_mm pulleyHelperMove = 10.0_mm; ///< Helper move for Load/Unload error states - when the MMU should slowly move the filament a bit static constexpr U_mm cutLength = 8.0_mm; -static constexpr U_mm fsensorToNozzle = 20.0_mm; /// ~20mm from MK4's filament sensor through extruder gears into nozzle +static constexpr U_mm fsensorToNozzle = 20.0_mm; ///< ~20mm from MK4's filament sensor through extruder gears into nozzle static constexpr U_mm fsensorToNozzleAvoidGrind = 5.0_mm; +/// Check the state of FSensor after this amount of filament got (hopefully) pulled out while unloading. +static constexpr U_mm fsensorUnloadCheckDistance = 20.0_mm; /// Begin: Pulley axis configuration static constexpr AxisConfig pulley = { diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index df71340..802c642 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -31,7 +31,7 @@ void UnloadFilament::Reset(uint8_t /*param*/) { ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off); } -void logic::UnloadFilament::FinishedCorrectly() { +void UnloadFilament::FinishedCorrectly() { state = ProgressCode::OK; error = ErrorCode::OK; mm::motion.Disable(mm::Pulley); @@ -39,21 +39,31 @@ void logic::UnloadFilament::FinishedCorrectly() { ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off); } +void UnloadFilament::GoToRetractingFromFinda() { + state = ProgressCode::RetractingFromFinda; + retract.Reset(); +} + +void UnloadFilament::GoToRecheckFilamentAgainstFINDA() { + state = ProgressCode::FeedingToFinda; + error = ErrorCode::RUNNING; + feed.Reset(true, true); +} + bool UnloadFilament::StepInner() { switch (state) { // state 1 engage idler - will be done by the Unload to FINDA state machine case ProgressCode::UnloadingToFinda: // state 2 rotate pulley as long as the FINDA is on if (unl.Step()) { - if (unl.State() == UnloadToFinda::Failed) { + if (unl.State() == UnloadToFinda::FailedFINDA) { // couldn't unload to FINDA, report error and wait for user to resolve it GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); - } else if (mfs::fsensor.Pressed()) { + } else if (unl.State() == UnloadToFinda::FailedFSensor) { // fsensor still pressed - that smells bad - a piece of filament may still be present in the heatsink // and that would cause serious problems while loading another filament GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_OFF); } else { - state = ProgressCode::RetractingFromFinda; - retract.Reset(); + GoToRetractingFromFinda(); } } return false; @@ -83,11 +93,17 @@ bool UnloadFilament::StepInner() { GoToErrEngagingIdler(); break; case mui::Event::Middle: // try again the whole sequence + // First invalidate homing flags as the user may have moved the Idler or Selector accidentally + InvalidateHoming(); if (mf::finda.Pressed()) { - Reset(0); + Reset(0); // filament is present in FINDA (regardless of FSensor) - assume we need to pull the filament to FINDA first + } else if (!mf::finda.Pressed() && mfs::fsensor.Pressed()) { + // a piece of filament is stuck in the extruder - keep waiting for the user to fix it } else { - state = ProgressCode::DisengagingIdler; - mi::idler.Disengage(); + // filament is not present in FINDA and not in FSensor + // - that means the filament can still be behind FINDA and blocking the selector + // Ideally push it to FINDA and then back to verify the whole situation + GoToRecheckFilamentAgainstFINDA(); } break; case mui::Event::Right: // problem resolved - the user pulled the fillament by hand @@ -104,8 +120,8 @@ bool UnloadFilament::StepInner() { error = ErrorCode::FINDA_DIDNT_SWITCH_OFF; state = ProgressCode::ERRWaitingForUser; // stand still } else { - // all sensors are ok - FinishedCorrectly(); + // all sensors are ok, but re-check the position of the filament against FINDA + GoToRecheckFilamentAgainstFINDA(); } break; default: @@ -129,6 +145,17 @@ bool UnloadFilament::StepInner() { GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); } return false; + case ProgressCode::FeedingToFinda: + // recovery mode - we assume the filament is somewhere between the idle position and FINDA - thus blocking the selector + if (feed.Step()) { + if (feed.State() == FeedToFinda::Failed) { + GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON); + } else { + state = ProgressCode::RetractingFromFinda; + retract.Reset(); + } + } + break; case ProgressCode::OK: return true; // successfully finished default: // we got into an unhandled state, better report it diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h index c3575be..51ef163 100644 --- a/src/logic/unload_filament.h +++ b/src/logic/unload_filament.h @@ -2,6 +2,7 @@ #pragma once #include #include "command_base.h" +#include "feed_to_finda.h" #include "unload_to_finda.h" #include "retract_from_finda.h" @@ -25,8 +26,11 @@ private: /// Common code for a correct completion of UnloadFilament void FinishedCorrectly(); + void GoToRetractingFromFinda(); + void GoToRecheckFilamentAgainstFINDA(); UnloadToFinda unl; + FeedToFinda feed; RetractFromFinda retract; }; diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp index c969326..0ea887d 100644 --- a/src/logic/unload_to_finda.cpp +++ b/src/logic/unload_to_finda.cpp @@ -1,6 +1,7 @@ /// @file unload_to_finda.cpp #include "unload_to_finda.h" #include "../modules/finda.h" +#include "../modules/fsensor.h" #include "../modules/globals.h" #include "../modules/idler.h" #include "../modules/leds.h" @@ -21,6 +22,11 @@ void UnloadToFinda::Reset(uint8_t maxTries) { } } +// @@TODO this may end up somewhere else as more code may need to check the distance traveled by the filament +int32_t CurrentPositionPulley_mm() { + return mm::stepsToUnit(mm::P_pos_t({ mm::motion.CurPosition(mm::Pulley) })); +} + bool UnloadToFinda::Step() { switch (state) { case EngagingIdler: @@ -29,18 +35,26 @@ bool UnloadToFinda::Step() { mm::motion.InitAxis(mm::Pulley); ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); } else { - state = Failed; + state = FailedFINDA; } return false; case UnloadingToFinda: if (mi::idler.Engaged()) { state = WaitingForFINDA; mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector); + unloadStart_mm = CurrentPositionPulley_mm(); mm::motion.PlanMove(-config::defaultBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, config::pulleyUnloadFeedrate); } return false; - case WaitingForFINDA: - if (!mf::finda.Pressed()) { + case WaitingForFINDA: { + int32_t currentPulley_mm = CurrentPositionPulley_mm(); + if ((abs(unloadStart_mm - currentPulley_mm) > config::fsensorUnloadCheckDistance.v) && 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()) { // detected end of filament state = OK; mm::motion.AbortPlannedMoves(); // stop rotating the pulley @@ -51,12 +65,14 @@ bool UnloadToFinda::Step() { if (--maxTries) { Reset(maxTries); // try again } else { - state = Failed; + state = FailedFINDA; } } + } return false; case OK: - case Failed: + case FailedFINDA: + case FailedFSensor: default: return true; } diff --git a/src/logic/unload_to_finda.h b/src/logic/unload_to_finda.h index 05bcf91..b84e516 100644 --- a/src/logic/unload_to_finda.h +++ b/src/logic/unload_to_finda.h @@ -18,7 +18,8 @@ struct UnloadToFinda { UnloadingToFinda, WaitingForFINDA, OK, - Failed + FailedFINDA, + FailedFSensor }; inline UnloadToFinda() : maxTries(3) {} @@ -36,6 +37,7 @@ struct UnloadToFinda { 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) }; } // namespace logic diff --git a/src/modules/axisunit.h b/src/modules/axisunit.h index 4ea42a7..faa59d7 100644 --- a/src/modules/axisunit.h +++ b/src/modules/axisunit.h @@ -112,13 +112,30 @@ static constexpr AU unitToAxisUnit(U v) { return { (typename AU::type_t)(v.v * axisScale[AU::axis].stepsPerUnit) }; } -/// Convert an unit::Unit to a steps type (pos_t or steps_t). +/// Convert an AxisUnit to unit::Unit. +/// The scaling factor is stored with the pair config::AxisConfig::uSteps and +/// config::AxisConfig::stepsPerUnit (one per-axis). +template +static constexpr typename U::type_t axisUnitToUnit(AU v) { + static_assert(AU::unit == U::unit, "incorrect unit type conversion"); + //static_assert(U::base == axisScale[AU::axis].base, "incorrect unit base conversion"); + return { (typename U::type_t)(v.v / axisScale[AU::axis].stepsPerUnit) }; +} + +/// Convert a unit::Unit to a steps type (pos_t or steps_t). /// Extract the raw step count from an AxisUnit with type checking. template static constexpr typename AU::type_t unitToSteps(U v) { return unitToAxisUnit(v).v; } +/// Convert a steps type (pos_t or steps_t) to a unit::Unit. +/// Extract the raw step count from an AxisUnit with type checking. +template +static constexpr typename U::type_t stepsToUnit(AU pos) { + return axisUnitToUnit(pos); +} + // Pulley typedef AxisUnit P_pos_t; ///< Pulley position type (steps) typedef AxisUnit P_speed_t; ///< Pulley speed type (steps/s) diff --git a/src/modules/usb_cdc.cpp b/src/modules/usb_cdc.cpp index 0515b59..15b8c34 100644 --- a/src/modules/usb_cdc.cpp +++ b/src/modules/usb_cdc.cpp @@ -82,7 +82,7 @@ void EVENT_CDC_Device_LineEncodingChanged(USB_ClassInfo_CDC_Device_t *const CDCI hal::watchdog::Enable(hal::watchdog::configuration::compute(250)); } } -} +} // extern "C" namespace modules { namespace usb { diff --git a/tests/unit/logic/stubs/main_loop_stub.cpp b/tests/unit/logic/stubs/main_loop_stub.cpp index 2ba39e1..53c5108 100644 --- a/tests/unit/logic/stubs/main_loop_stub.cpp +++ b/tests/unit/logic/stubs/main_loop_stub.cpp @@ -141,3 +141,18 @@ void SetFINDAStateAndDebounce(bool press) { for (size_t i = 0; i < config::findaDebounceMs + 1; ++i) main_loop(); } + +// The idea is to set fsOff and findaOff to some reasonable values (like 10 and 1000) +// for normal situations. +// For errorneous situations set fsOff or findaOff to some number higher than the number of steps +// the testing routine is allowed to do -> thus effectively blocking the corresponding moment for fsensor +// and finda switching off +bool SimulateUnloadToFINDA(uint32_t step, uint32_t fsOff, uint32_t findaOff) { + if (step == fsOff) { // make FSensor switch off + mfs::fsensor.ProcessMessage(false); + return true; + } else if (step == findaOff) { // make FINDA switch off + hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); + } + return mf::finda.Pressed(); +} diff --git a/tests/unit/logic/stubs/main_loop_stub.h b/tests/unit/logic/stubs/main_loop_stub.h index 7c8c3ac..73cfcd0 100644 --- a/tests/unit/logic/stubs/main_loop_stub.h +++ b/tests/unit/logic/stubs/main_loop_stub.h @@ -27,6 +27,7 @@ void SetFINDAStateAndDebounce(bool press); void SimulateIdlerHoming(); void SimulateSelectorHoming(); void SimulateIdlerAndSelectorHoming(); +bool SimulateUnloadToFINDA(uint32_t step, uint32_t fsOff, uint32_t findaOff); // these are recommended max steps for simulated movement of the idler and selector // - roughly the amount of motion steps from one end to the other + some margin diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index f95d0a1..3038b27 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -48,6 +48,7 @@ pos_t Motion::Position(Axis axis) const { void Motion::SetPosition(Axis axis, pos_t x) { axes[axis].pos = x; + axisData[axis].ctrl.SetPosition(axes[axis].pos); } void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) { @@ -58,6 +59,7 @@ st_timer_t Motion::Step() { if (axes[i].pos != axes[i].targetPos) { int8_t dirInc = (axes[i].pos < axes[i].targetPos) ? 1 : -1; axes[i].pos += dirInc; + axisData[i].ctrl.SetPosition(axes[i].pos); } } return 0; @@ -83,6 +85,7 @@ void Motion::AbortPlannedMoves(bool halt) { void Motion::AbortPlannedMoves(config::Axis i, bool) { axes[i].targetPos = axes[i].pos; // leave the axis where it was at the time of abort + axisData[i].ctrl.SetPosition(axes[i].pos); } void ReinitMotion() { diff --git a/tests/unit/logic/unload_filament/test_unload_filament.cpp b/tests/unit/logic/unload_filament/test_unload_filament.cpp index ef1f152..8cfc070 100644 --- a/tests/unit/logic/unload_filament/test_unload_filament.cpp +++ b/tests/unit/logic/unload_filament/test_unload_filament.cpp @@ -18,6 +18,7 @@ #include "../stubs/stub_motion.h" using Catch::Matchers::Equals; +using namespace std::placeholders; #include "../helpers/helpers.ipp" @@ -38,7 +39,7 @@ void RegularUnloadFromSlot04Init(uint8_t slot, logic::UnloadFilament &uf) { uf.Reset(slot); } -void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) { +void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf, uint8_t entryIdlerSlotIndex, bool selectorShallHomeAtEnd) { // Stage 0 - verify state just after Reset() // we still think we have filament loaded at this stage // idler should have been activated by the underlying automaton @@ -46,7 +47,7 @@ void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) { // FINDA on // green LED should blink, red off REQUIRE(VerifyState(uf, (mg::FilamentLoadState)(mg::FilamentLoadState::InNozzle | mg::FilamentLoadState::InSelector), - mi::Idler::IdleSlotIndex(), slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda)); + entryIdlerSlotIndex, slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda)); // run the automaton // Stage 1 - unloading to FINDA @@ -72,6 +73,10 @@ void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) { // Stage 3 - idler was engaged, disengage it REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps)); + if (selectorShallHomeAtEnd) { + SimulateSelectorHoming(); + } + // filament unloaded // idler should have been disengaged // no change in selector's position @@ -90,7 +95,7 @@ TEST_CASE("unload_filament::regular_unload_from_slot_0-4", "[unload_filament]") for (uint8_t slot = 0; slot < config::toolCount; ++slot) { logic::UnloadFilament uf; RegularUnloadFromSlot04Init(slot, uf); - RegularUnloadFromSlot04(slot, uf); + RegularUnloadFromSlot04(slot, uf, mi::Idler::IdleSlotIndex(), false); } } @@ -125,7 +130,13 @@ void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) { // run the automaton // Stage 1 - unloading to FINDA - do NOT let it trigger - keep it pressed, the automaton should finish all moves with the pulley // without reaching the FINDA and report an error - REQUIRE(WhileTopState(uf, ProgressCode::UnloadingToFinda, 500000)); + REQUIRE(WhileCondition( + uf, + [&](uint32_t step) { + SimulateUnloadToFINDA(step, 10, 1'000'000); + return uf.TopLevelState() == ProgressCode::UnloadingToFinda; + }, + 200'000)); // we still think we have filament loaded at this stage // idler should have been activated by the underlying automaton @@ -243,6 +254,9 @@ void FindaDidntTriggerResolveTryAgain(uint8_t slot, logic::UnloadFilament &uf) { // FINDA still on // red LED should blink, green LED should be off REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToFinda)); + + // Assume, the Idler homed (homing is invalidated after pressing the recovery button) + SimulateIdlerHoming(); } TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_filament]") { @@ -250,7 +264,7 @@ TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_fil logic::UnloadFilament uf; FindaDidntTriggerCommonSetup(slot, uf); FindaDidntTriggerResolveTryAgain(slot, uf); - RegularUnloadFromSlot04(slot, uf); + RegularUnloadFromSlot04(slot, uf, slot, true); } } @@ -281,6 +295,19 @@ void FailedUnloadResolveManual(uint8_t slot, logic::UnloadFilament &uf) { hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); PressButtonAndDebounce(uf, mb::Right); + REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda)); + + // we still need to feed to FINDA and back to verify the position of the filament + SimulateIdlerHoming(); + + REQUIRE(WhileTopState(uf, ProgressCode::FeedingToFinda, 5000)); + + REQUIRE(WhileTopState(uf, ProgressCode::RetractingFromFinda, idlerEngageDisengageMaxSteps)); + + REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps)); + + SimulateSelectorHoming(); + REQUIRE(VerifyState(uf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); } 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 712a5bc..31b227a 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 @@ -1,5 +1,7 @@ #include "catch2/catch.hpp" +#include + #include "../../../../src/modules/buttons.h" #include "../../../../src/modules/finda.h" #include "../../../../src/modules/fsensor.h" @@ -18,6 +20,7 @@ #include "../stubs/stub_motion.h" using Catch::Matchers::Equals; +using namespace std::placeholders; namespace ha = hal::adc; @@ -27,6 +30,8 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") { // we need finda ON SetFINDAStateAndDebounce(true); + // fsensor should be ON + mfs::fsensor.ProcessMessage(true); // and MMU "thinks" it has the filament loaded mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle); @@ -51,11 +56,7 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") { // now pulling the filament until finda triggers REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA); - hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); - REQUIRE(WhileCondition( - ff, - [&](uint32_t) { return mf::finda.Pressed(); }, - 50000)); + REQUIRE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 10, 1000), 1100)); REQUIRE(ff.State() == logic::UnloadToFinda::OK); REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector); @@ -81,6 +82,8 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]") // we need finda ON SetFINDAStateAndDebounce(true); + // fsensor should be ON + mfs::fsensor.ProcessMessage(true); // and MMU "thinks" it has the filament loaded mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle); @@ -107,11 +110,50 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]") REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA); // no changes to FINDA during unload - we'll pretend it never triggers - REQUIRE_FALSE(WhileCondition( - ff, - [&](uint32_t) { return mf::finda.Pressed(); }, - 50000)); + // but set FSensor correctly + REQUIRE_FALSE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 10, 150000), 50000)); - REQUIRE(ff.State() == logic::UnloadToFinda::Failed); + REQUIRE(ff.State() == logic::UnloadToFinda::FailedFINDA); + REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector); +} + +TEST_CASE("unload_to_finda::unload_without_FSensor_trigger", "[unload_to_finda]") { + ForceReinitAllAutomata(); + EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley); + + // we need finda ON + SetFINDAStateAndDebounce(true); + // fsensor should be ON + mfs::fsensor.ProcessMessage(true); + // and MMU "thinks" it has the filament loaded + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle); + + logic::UnloadToFinda ff; + + // restart the automaton - just 1 attempt + ff.Reset(1); + + REQUIRE(ff.State() == logic::UnloadToFinda::EngagingIdler); + + // it should have instructed the selector and idler to move to slot 1 + // check if the idler and selector have the right command + CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v); + CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v); + CHECK(mm::axes[mm::Idler].enabled == true); + + // engaging idler + REQUIRE(WhileCondition( + ff, + [&](uint32_t) { return !mi::idler.Engaged(); }, + 5000)); + + // now pulling the filament until finda triggers + REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA); + + // no changes to FSensor during unload - we'll pretend it never triggers + // but set FINDA correctly + REQUIRE(WhileCondition(ff, std::bind(SimulateUnloadToFINDA, _1, 150000, 10000), 50000)); + + REQUIRE(ff.State() == logic::UnloadToFinda::FailedFSensor); REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InSelector); }