diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp index a95b1b6..a3afe1a 100644 --- a/src/logic/command_base.cpp +++ b/src/logic/command_base.cpp @@ -117,6 +117,7 @@ void CommandBase::GoToErrDisengagingIdler(ErrorCode ec) { void CommandBase::GoToErrEngagingIdler() { state = ProgressCode::ERREngagingIdler; + error = ErrorCode::RUNNING; mi::idler.Engage(mg::globals.ActiveSlot()); } diff --git a/src/logic/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp index c1d4d9d..130883b 100644 --- a/src/logic/feed_to_bondtech.cpp +++ b/src/logic/feed_to_bondtech.cpp @@ -19,6 +19,14 @@ void FeedToBondtech::Reset(uint8_t maxRetries) { mi::idler.Engage(mg::globals.ActiveSlot()); } +void logic::FeedToBondtech::GoToPushToNozzle() { + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InFSensor); + // plan a slow move to help push filament into the nozzle + //@@TODO the speed in mm/s must correspond to printer's feeding speed! + mm::motion.PlanMove(config::fsensorToNozzle, config::pulleySlowFeedrate); + state = PushingFilamentIntoNozzle; +} + bool FeedToBondtech::Step() { switch (state) { case EngagingIdler: @@ -34,11 +42,7 @@ bool FeedToBondtech::Step() { //dbg_logic_P(PSTR("Feed to Bondtech --> Pushing")); if (mfs::fsensor.Pressed()) { mm::motion.AbortPlannedMoves(); // stop pushing filament - mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InFSensor); - // plan a slow move to help push filament into the nozzle - //@@TODO the speed in mm/s must correspond to printer's feeding speed! - mm::motion.PlanMove(config::fsensorToNozzle, config::pulleySlowFeedrate); - state = PushingFilamentIntoNozzle; + GoToPushToNozzle(); } else if (mm::motion.StallGuard(mm::Pulley)) { // stall guard occurred during movement - the filament got stuck state = Failed; // @@TODO may be even report why it failed diff --git a/src/logic/feed_to_bondtech.h b/src/logic/feed_to_bondtech.h index 71802a6..b4a28bc 100644 --- a/src/logic/feed_to_bondtech.h +++ b/src/logic/feed_to_bondtech.h @@ -36,6 +36,11 @@ struct FeedToBondtech { /// @returns Failed if the maximum feed length has been reached without the the printer's fsensor trigger being reported inline uint8_t State() const { return state; } + /// Transition to PushingFilamentIntoNozzle. + /// Allows jumping into the state machine from outside just to push filament slowly into the nozzle + /// (this principle uses tool change) + void GoToPushToNozzle(); + private: uint8_t state; uint8_t maxRetries; diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp index dc40783..651984c 100644 --- a/src/logic/tool_change.cpp +++ b/src/logic/tool_change.cpp @@ -44,6 +44,21 @@ void ToolChange::Reset(uint8_t param) { } } +void logic::ToolChange::GoToFeedingToBondtech() { + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); + james.Reset(3); + state = ProgressCode::FeedingToBondtech; + error = ErrorCode::RUNNING; +} + +void logic::ToolChange::FinishedCorrectly() { + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on); + state = ProgressCode::OK; + error = ErrorCode::OK; +} + bool ToolChange::StepInner() { switch (state) { case ProgressCode::UnloadingFilament: @@ -62,8 +77,7 @@ bool ToolChange::StepInner() { if (feed.State() == FeedToFinda::Failed) { GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON); // signal loading error } else { - state = ProgressCode::FeedingToBondtech; - james.Reset(3); + GoToFeedingToBondtech(); } } break; @@ -72,15 +86,13 @@ bool ToolChange::StepInner() { if (james.State() == FeedToBondtech::Failed) { GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_ON); // signal loading error } else { - state = ProgressCode::OK; - error = ErrorCode::OK; + FinishedCorrectly(); } } break; case ProgressCode::OK: return true; - // @@TODO error handling definitely needs unifying with the LoadFilament state machine case ProgressCode::ERRDisengagingIdler: ErrDisengagingIdler(); return false; @@ -106,10 +118,7 @@ bool ToolChange::StepInner() { state = ProgressCode::ERRWaitingForUser; // stand still } else { // all sensors are ok - ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off); - ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on); - state = ProgressCode::OK; - error = ErrorCode::OK; + FinishedCorrectly(); } break; default: // no event, continue waiting for user input @@ -127,14 +136,13 @@ bool ToolChange::StepInner() { // @@TODO helping filament needs improvement - the filament should try to move forward as long as the button is pressed if (mf::finda.Pressed()) { // the help was enough to press the FINDA, we are ok, continue normally - state = ProgressCode::FeedingToBondtech; - error = ErrorCode::RUNNING; + GoToFeedingToBondtech(); } else if (mfs::fsensor.Pressed()) { // the help was enough to press the filament sensor, we are ok, continue normally - // This is not correct @@TODO - when the fsensor triggers, we still need to push the filament into the nozzle/gears + GoToFeedingToBondtech(); + // Beware, when the fsensor triggers, we still need to push the filament into the nozzle/gears // which requires restarting James from its last stage - state = ProgressCode::FeedingToBondtech; - error = ErrorCode::RUNNING; + james.GoToPushToNozzle(); } else if (mm::motion.QueueEmpty()) { // helped a bit, but FINDA/Fsensor didn't trigger, return to the main error state GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_ON); diff --git a/src/logic/tool_change.h b/src/logic/tool_change.h index 06cf587..0703e45 100644 --- a/src/logic/tool_change.h +++ b/src/logic/tool_change.h @@ -26,6 +26,11 @@ public: ErrorCode Error() const override; private: + void GoToFeedingToBondtech(); + + /// Common code for a correct completion of UnloadFilament + void FinishedCorrectly(); + UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well FeedToFinda feed; FeedToBondtech james; // bond ;) diff --git a/tests/unit/logic/load_filament/test_load_filament.cpp b/tests/unit/logic/load_filament/test_load_filament.cpp index fb730ff..3be0aa5 100644 --- a/tests/unit/logic/load_filament/test_load_filament.cpp +++ b/tests/unit/logic/load_filament/test_load_filament.cpp @@ -101,12 +101,12 @@ void FailedLoadToFindaResolveHelp(uint8_t slot, logic::LoadFilament &lf) { // In this case we check the first option PressButtonAndDebounce(lf, mb::Left); - REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERREngagingIdler)); + REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERREngagingIdler)); // Stage 4 - engage the idler REQUIRE(WhileTopState(lf, ProgressCode::ERREngagingIdler, idlerEngageDisengageMaxSteps)); - REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRHelpingFilament)); + REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament)); } void FailedLoadToFindaResolveHelpFindaTriggered(uint8_t slot, logic::LoadFilament &lf) { diff --git a/tests/unit/logic/tool_change/test_tool_change.cpp b/tests/unit/logic/tool_change/test_tool_change.cpp index c448cda..d553735 100644 --- a/tests/unit/logic/tool_change/test_tool_change.cpp +++ b/tests/unit/logic/tool_change/test_tool_change.cpp @@ -21,13 +21,15 @@ using Catch::Matchers::Equals; #include "../helpers/helpers.ipp" -void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot) { +void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot, uint32_t triggerAt = 1000) { // feeding to finda REQUIRE(WhileCondition( tc, [&](int step) -> bool { - if(step == 1000){ // on 1000th step make FINDA trigger + if(step == triggerAt){ // on specified stepNr make FINDA trigger hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); + } else if(step >= triggerAt + config::findaDebounceMs + 1){ + REQUIRE(mf::finda.Pressed() == true); } return tc.TopLevelState() == ProgressCode::FeedingToFinda; }, 200000UL)); @@ -47,7 +49,13 @@ void FeedingToBondtech(logic::ToolChange &tc, uint8_t toSlot) { REQUIRE(VerifyState(tc, mg::FilamentLoadState::InNozzle, mi::Idler::IdleSlotIndex(), toSlot, true, false, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK)); } -void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { +void CheckFinishedCorrectly(logic::ToolChange &tc, uint8_t toSlot) { + REQUIRE(tc.TopLevelState() == ProgressCode::OK); + REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle); + REQUIRE(mg::globals.ActiveSlot() == toSlot); +} + +void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) { ForceReinitAllAutomata(); EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle); @@ -63,20 +71,23 @@ void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { } return tc.TopLevelState() == ProgressCode::UnloadingFilament; }, 200000UL)); - CHECKED_ELSE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley) { - ++toSlot; - } + + auto gf = ml::leds.Mode(fromSlot, ml::green); + auto gt = ml::leds.Mode(toSlot, ml::green); + auto rf = ml::leds.Mode(fromSlot, ml::red); + auto rt = ml::leds.Mode(toSlot, ml::red); + + // REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley); + REQUIRE(VerifyState2(tc, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), fromSlot, false, false, toSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda)); FeedingToFinda(tc, toSlot); FeedingToBondtech(tc, toSlot); - REQUIRE(tc.TopLevelState() == ProgressCode::OK); - REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle); - REQUIRE(mg::globals.ActiveSlot() == toSlot); + CheckFinishedCorrectly(tc, toSlot); } -void NoToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { +void NoToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) { ForceReinitAllAutomata(); // the filament is LOADED @@ -92,7 +103,7 @@ void NoToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { REQUIRE(tc.Error() == ErrorCode::OK); } -void JustLoadFilament(logic::ToolChange tc, uint8_t slot) { +void JustLoadFilament(logic::ToolChange &tc, uint8_t slot) { ForceReinitAllAutomata(); EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley); @@ -107,9 +118,7 @@ void JustLoadFilament(logic::ToolChange tc, uint8_t slot) { FeedingToBondtech(tc, slot); - REQUIRE(tc.TopLevelState() == ProgressCode::OK); - REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle); - REQUIRE(mg::globals.ActiveSlot() == slot); + CheckFinishedCorrectly(tc, slot); } TEST_CASE("tool_change::test0", "[tool_change]") { @@ -154,3 +163,116 @@ TEST_CASE("tool_change::same_slot_just_unloaded_filament", "[tool_change]") { JustLoadFilament(tc, toSlot); } } + +void ToolChangeFailLoadToFinda(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) { + ForceReinitAllAutomata(); + + EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle); + + // restart the automaton + tc.Reset(toSlot); + + REQUIRE(WhileCondition( + tc, + [&](int step) -> bool { + if(step == 2000){ // on 2000th step make FINDA trigger + hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); + } + return tc.TopLevelState() == ProgressCode::UnloadingFilament; }, + 200000UL)); + + REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley); + + // feeding to finda, but fails - do not trigger FINDA + REQUIRE(WhileTopState(tc, ProgressCode::FeedingToFinda, 50000UL)); + + // should end up in error disengage idler + REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRDisengagingIdler)); + REQUIRE(WhileTopState(tc, ProgressCode::ERRDisengagingIdler, 5000)); +} + +void ToolChangeFailLoadToFindaButton0(logic::ToolChange &tc, uint8_t toSlot) { + // now waiting for user input + PressButtonAndDebounce(tc, 0); + + REQUIRE(WhileTopState(tc, ProgressCode::ERREngagingIdler, 5000UL)); + + REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament)); + + // try push more, if FINDA triggers, continue loading + REQUIRE(WhileCondition( + tc, + [&](int step) -> bool { + if(step == 20){ // on 20th step make FINDA trigger + hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); + } + return tc.TopLevelState() == ProgressCode::ERRHelpingFilament; }, + 2000UL)); + + REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToBondtech)); + + FeedingToBondtech(tc, toSlot); + + CheckFinishedCorrectly(tc, toSlot); +} + +void ToolChangeFailLoadToFindaButton1(logic::ToolChange &tc, uint8_t toSlot) { + // now waiting for user input + PressButtonAndDebounce(tc, 1); + + REQUIRE(WhileCondition( + tc, + [&](int step) -> bool { + if(step == 2000){ // on 2000th step make FINDA trigger + hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); + } + return tc.TopLevelState() == ProgressCode::UnloadingFilament; }, + 200000UL)); + REQUIRE(VerifyState(tc, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), toSlot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda)); + + // retry the whole operation + // beware - the FeedToFinda state machine will leverage the already engaged Idler, + // so the necessary number of steps to reach the FINDA is quite low (~200 was lowest once tested) + // without running short of max distance of Pulley to travel + FeedingToFinda(tc, toSlot, 200); + + FeedingToBondtech(tc, toSlot); + + CheckFinishedCorrectly(tc, toSlot); +} + +void ToolChangeFailLoadToFindaButton2(logic::ToolChange &tc, uint8_t toSlot) { + // now waiting for user input + PressButtonAndDebounce(tc, 2); + + // the user resolved the situation by hand + FeedingToFinda(tc, toSlot); + + FeedingToBondtech(tc, toSlot); + + CheckFinishedCorrectly(tc, toSlot); +} + +TEST_CASE("tool_change::load_fail_FINDA_resolve_btn0", "[tool_change]") { + logic::ToolChange tc; + for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) { + for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) { + if (fromSlot != toSlot) { + ToolChangeFailLoadToFinda(tc, fromSlot, toSlot); + ToolChangeFailLoadToFindaButton0(tc, toSlot); + } + } + } +} + +TEST_CASE("tool_change::load_fail_FINDA_resolve_btn1", "[tool_change]") { + logic::ToolChange tc; + for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) { + for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) { + if (fromSlot != toSlot) { + ToolChangeFailLoadToFinda(tc, fromSlot, toSlot); + ToolChangeFailLoadToFindaButton1(tc, toSlot); + } + } + } +} diff --git a/tests/unit/logic/unload_filament/test_unload_filament.cpp b/tests/unit/logic/unload_filament/test_unload_filament.cpp index 2988973..6e94069 100644 --- a/tests/unit/logic/unload_filament/test_unload_filament.cpp +++ b/tests/unit/logic/unload_filament/test_unload_filament.cpp @@ -164,7 +164,7 @@ void FindaDidntTriggerResolveHelp(uint8_t slot, logic::UnloadFilament &uf) { // no change in selector's position // FINDA still on // red LED should blink, green LED should be off - REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERREngagingIdler)); + REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERREngagingIdler)); // Stage 4 - engage the idler REQUIRE(WhileTopState(uf, ProgressCode::ERREngagingIdler, idlerEngageDisengageMaxSteps)); @@ -174,7 +174,7 @@ void FindaDidntTriggerResolveHelp(uint8_t slot, logic::UnloadFilament &uf) { // no change in selector's position // FINDA still on // red LED should blink, green LED should be off - REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERRHelpingFilament)); + REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament)); } void FindaDidntTriggerResolveHelpFindaTriggered(uint8_t slot, logic::UnloadFilament &uf) {