From bbd14ed217089f4cb16c308d49e9af8fea04a8e7 Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Mon, 27 Sep 2021 09:49:14 +0200 Subject: [PATCH] Fix ToolChange operation + unit tests --- src/logic/load_filament.cpp | 7 +- src/logic/tool_change.cpp | 99 +++++++++++++++---- src/logic/tool_change.h | 6 +- src/logic/unload_filament.cpp | 5 +- .../logic/tool_change/test_tool_change.cpp | 48 ++++++--- 5 files changed, 121 insertions(+), 44 deletions(-) diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index 5f6fbe4..f279527 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -81,11 +81,8 @@ bool LoadFilament::StepInner() { break; case ProgressCode::OK: return true; - case ProgressCode::ERRDisengagingIdler: // couldn't unload to FINDA - if (!mi::idler.Engaged()) { - state = ProgressCode::ERRWaitingForUser; - mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input - } + case ProgressCode::ERRDisengagingIdler: // couldn't load to FINDA + ErrDisengagingIdler(); return false; case ProgressCode::ERRWaitingForUser: { // waiting for user buttons and/or a command from the printer diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp index 7773086..982aac7 100644 --- a/src/logic/tool_change.cpp +++ b/src/logic/tool_change.cpp @@ -7,6 +7,7 @@ #include "../modules/motion.h" #include "../modules/permanent_storage.h" #include "../modules/selector.h" +#include "../modules/user_input.h" #ifdef DEBUG_LOGIC #include "../hal/usart.h" #include @@ -41,11 +42,13 @@ void ToolChange::Reset(uint8_t param) { state = ProgressCode::UnloadingFilament; unl.Reset(mg::globals.ActiveSlot()); } else { - state = ProgressCode::LoadingFilament; + state = ProgressCode::FeedingToFinda; + error = ErrorCode::RUNNING; #ifdef DEBUG_LOGIC hu::usart1.puts("Filament is not loaded --> load\n"); #endif //DEBUG_LOGIC - load.Reset(plannedSlot); + mg::globals.SetActiveSlot(plannedSlot); + feed.Reset(true); } } @@ -56,21 +59,85 @@ bool ToolChange::StepInner() { // unloading sequence finished - basically, no errors can occurr here // as UnloadFilament should handle all the possible error states on its own // There is no way the UnloadFilament to finish in an error state - state = ProgressCode::LoadingFilament; - load.Reset(plannedSlot); + state = ProgressCode::FeedingToFinda; + error = ErrorCode::RUNNING; + feed.Reset(true); } break; - case ProgressCode::LoadingFilament: - if (load.StepInner()) { - // loading sequence finished - basically, no errors can occurr here - // as LoadFilament should handle all the possible error states on its own - // There is no way the LoadFilament to finish in an error state - state = ProgressCode::OK; - error = ErrorCode::OK; + case ProgressCode::FeedingToFinda: + if (feed.Step()) { + if (feed.State() == FeedToFinda::Failed) { + state = ProgressCode::ERRDisengagingIdler; + error = ErrorCode::FINDA_DIDNT_SWITCH_ON; + mi::idler.Disengage(); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0); // signal loading error + } else { + state = ProgressCode::FeedingToBondtech; + james.Reset(3); + } + } + break; + case ProgressCode::FeedingToBondtech: + if (james.Step()) { + if (james.State() == FeedToBondtech::Failed) { + state = ProgressCode::ERRDisengagingIdler; + error = ErrorCode::FSENSOR_DIDNT_SWITCH_ON; + mi::idler.Disengage(); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0); // signal loading error + } else { + mg::globals.SetFilamentLoaded(true); + state = ProgressCode::OK; + error = ErrorCode::OK; + } } break; case ProgressCode::OK: return true; + + // @@TODO error handling definitely needs unifying with the LoadFilament state machine + case ProgressCode::ERRDisengagingIdler: + ErrDisengagingIdler(); + return false; + case ProgressCode::ERRWaitingForUser: { + // waiting for user buttons and/or a command from the printer + mui::Event ev = mui::userInput.ConsumeEvent(); + switch (ev) { + case mui::Event::Left: // try to manually load just a tiny bit - help the filament with the pulley + state = ProgressCode::ERREngagingIdler; + mi::idler.Engage(mg::globals.ActiveSlot()); + break; + case mui::Event::Middle: // try again the whole sequence + Reset(mg::globals.ActiveSlot()); + break; + case mui::Event::Right: // problem resolved - the user pushed the fillament by hand? + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on); + // mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants + state = ProgressCode::AvoidingGrind; + break; + default: // no event, continue waiting for user input + break; + } + return false; + } + case ProgressCode::ERREngagingIdler: + if (mi::idler.Engaged()) { + state = ProgressCode::ERRHelpingFilament; + mm::motion.PlanMove(mm::Pulley, 450, 5000); //@@TODO constants + } + return false; + case ProgressCode::ERRHelpingFilament: + if (mf::finda.Pressed()) { + // the help was enough to press the FINDA, we are ok, continue normally + state = ProgressCode::FeedingToBondtech; + error = ErrorCode::RUNNING; + } else if (mm::motion.QueueEmpty()) { + // helped a bit, but FINDA didn't trigger, return to the main error state + state = ProgressCode::ERRDisengagingIdler; + } + return false; default: // we got into an unhandled state, better report it state = ProgressCode::ERRInternal; error = ErrorCode::INTERNAL; @@ -83,8 +150,6 @@ ProgressCode ToolChange::State() const { switch (state) { case ProgressCode::UnloadingFilament: return unl.State(); // report sub-automaton states properly - case ProgressCode::LoadingFilament: - return load.State(); // report sub-automaton states properly default: return state; } @@ -92,10 +157,10 @@ ProgressCode ToolChange::State() const { ErrorCode ToolChange::Error() const { switch (state) { - case ProgressCode::UnloadingFilament: - return unl.Error(); // report sub-automaton errors properly - case ProgressCode::LoadingFilament: - return load.Error(); // report sub-automaton errors properly + case ProgressCode::UnloadingFilament: { + ErrorCode ec = unl.Error(); // report sub-automaton errors properly, only filter out OK and replace them with RUNNING + return ec == ErrorCode::OK ? ErrorCode::RUNNING : ec; + } default: return error; } diff --git a/src/logic/tool_change.h b/src/logic/tool_change.h index e58cf34..2d43328 100644 --- a/src/logic/tool_change.h +++ b/src/logic/tool_change.h @@ -2,7 +2,8 @@ #include #include "command_base.h" #include "unload_filament.h" -#include "load_filament.h" +#include "feed_to_finda.h" +#include "feed_to_bondtech.h" namespace logic { @@ -25,7 +26,8 @@ public: private: UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well - LoadFilament load; + FeedToFinda feed; + FeedToBondtech james; // bond ;) uint8_t plannedSlot; }; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 2a12d96..e20a73d 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -69,10 +69,7 @@ bool UnloadFilament::StepInner() { } return false; case ProgressCode::ERRDisengagingIdler: // couldn't unload to FINDA - if (!mi::idler.Engaged()) { - state = ProgressCode::ERRWaitingForUser; - mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input - } + ErrDisengagingIdler(); return false; case ProgressCode::ERRWaitingForUser: { // waiting for user buttons and/or a command from the printer diff --git a/tests/unit/logic/tool_change/test_tool_change.cpp b/tests/unit/logic/tool_change/test_tool_change.cpp index 53dc266..a99c171 100644 --- a/tests/unit/logic/tool_change/test_tool_change.cpp +++ b/tests/unit/logic/tool_change/test_tool_change.cpp @@ -21,6 +21,32 @@ using Catch::Matchers::Equals; #include "../helpers/helpers.ipp" +void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot) { + // feeding to finda + REQUIRE(WhileCondition( + tc, + [&](int step) -> bool { + if(step == 1000){ // on 1000th step make FINDA trigger + hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); + } + return tc.TopLevelState() == ProgressCode::FeedingToFinda; }, + 200000UL)); + REQUIRE(VerifyState(tc, false, toSlot, toSlot, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToBondtech)); +} + +void FeedingToBondtech(logic::ToolChange &tc, uint8_t toSlot) { + // james is feeding + REQUIRE(WhileCondition( + tc, + [&](int step) -> bool { + if(step == 5000){ // on 5000th step make filament sensor trigger + mfs::fsensor.ProcessMessage(true); + } + return tc.TopLevelState() == ProgressCode::FeedingToBondtech; }, + 200000UL)); + REQUIRE(VerifyState(tc, true, mi::Idler::IdleSlotIndex(), toSlot, true, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK)); +} + void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { ForceReinitAllAutomata(); @@ -39,14 +65,9 @@ void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) { 200000UL)); REQUIRE(mg::globals.FilamentLoaded() == false); - REQUIRE(WhileCondition( - tc, - [&](int step) -> bool { - if(step == 1000){ // on 1000th step make FINDA trigger - hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); - } - return tc.TopLevelState() == ProgressCode::LoadingFilament; }, - 200000UL)); + FeedingToFinda(tc, toSlot); + + FeedingToBondtech(tc, toSlot); REQUIRE(tc.TopLevelState() == ProgressCode::OK); REQUIRE(mg::globals.FilamentLoaded() == true); @@ -82,14 +103,9 @@ void JustLoadFilament(logic::ToolChange tc, uint8_t slot) { // restart the automaton tc.Reset(slot); - REQUIRE(WhileCondition( - tc, - [&](int step) -> bool { - if(step == 1000){ // on 1000th step make FINDA trigger - hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); - } - return tc.TopLevelState() == ProgressCode::LoadingFilament; }, - 200000UL)); + FeedingToFinda(tc, slot); + + FeedingToBondtech(tc, slot); REQUIRE(tc.TopLevelState() == ProgressCode::OK); REQUIRE(mg::globals.FilamentLoaded() == true);