diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h index 98c8175..9cdb46d 100644 --- a/src/logic/progress_codes.h +++ b/src/logic/progress_codes.h @@ -17,8 +17,10 @@ enum class ProgressCode : uint_fast8_t { FinishingMoves, ERR1DisengagingIdler, + ERR1EngagingIdler, ERR1WaitingForUser, ERRInternal, + ERR1HelpingFilament, UnloadingFilament, LoadingFilament, diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 1c31a12..d632493 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -13,6 +13,7 @@ UnloadFilament unloadFilament; namespace mb = modules::buttons; namespace mm = modules::motion; +namespace mf = modules::finda; namespace mi = modules::idler; namespace ml = modules::leds; namespace mg = modules::globals; @@ -79,7 +80,8 @@ bool UnloadFilament::Step() { bool userResolved = mb::buttons.ButtonPressed(mb::Right) /*|| command_userResolved()*/; if (help) { // try to manually unload just a tiny bit - help the filament with the pulley - //@@TODO + state = ProgressCode::ERR1EngagingIdler; + mi::idler.Engage(mg::globals.ActiveSlot()); } else if (tryAgain) { // try again the whole sequence Reset(0); @@ -92,6 +94,22 @@ bool UnloadFilament::Step() { } return false; } + case ProgressCode::ERR1EngagingIdler: + if (mi::idler.Engaged()) { + state = ProgressCode::ERR1HelpingFilament; + mm::motion.PlanMove(mm::Pulley, 450, 5000); + } + return false; + case ProgressCode::ERR1HelpingFilament: + if (!mf::finda.Pressed()) { + // the help was enough to depress the FINDA, we are ok, continue normally + state = ProgressCode::DisengagingIdler; + error = ErrorCode::OK; + } else if (mm::motion.QueueEmpty()) { + // helped a bit, but FINDA didn't trigger, return to the main error state + state = ProgressCode::ERR1DisengagingIdler; + } + return false; case ProgressCode::OK: return true; // successfully finished default: // we got into an unhandled state, better report it diff --git a/tests/unit/logic/stubs/main_loop_stub.cpp b/tests/unit/logic/stubs/main_loop_stub.cpp index d115b47..401bf8b 100644 --- a/tests/unit/logic/stubs/main_loop_stub.cpp +++ b/tests/unit/logic/stubs/main_loop_stub.cpp @@ -16,6 +16,7 @@ #include "../stubs/stub_motion.h" #include // bring in placement new +#include void main_loop() { modules::buttons::buttons.Step(); @@ -65,3 +66,18 @@ void ForceReinitAllAutomata() { modules::globals::globals.SetFilamentLoaded(false); modules::globals::globals.SetActiveSlot(0); } + +void EnsureActiveSlotIndex(uint8_t slot) { + // move selector to the right spot + modules::selector::selector.MoveToSlot(slot); + while (modules::selector::selector.Slot() != slot) + main_loop(); + + modules::globals::globals.SetActiveSlot(slot); +} + +void SetFINDAStateAndDebounce(bool press) { + hal::adc::SetADC(1, press ? modules::finda::FINDA::adcDecisionLevel + 1 : modules::finda::FINDA::adcDecisionLevel - 1); + for (size_t i = 0; i < modules::finda::FINDA::debounce + 1; ++i) + main_loop(); +} diff --git a/tests/unit/logic/stubs/main_loop_stub.h b/tests/unit/logic/stubs/main_loop_stub.h index a22d664..f1d804f 100644 --- a/tests/unit/logic/stubs/main_loop_stub.h +++ b/tests/unit/logic/stubs/main_loop_stub.h @@ -6,9 +6,11 @@ extern void ForceReinitAllAutomata(); template bool WhileCondition(SM &sm, COND cond, uint32_t maxLoops = 5000) { - while (cond(maxLoops) && --maxLoops) { + uint32_t step = 0; + while (cond(step) && --maxLoops) { main_loop(); sm.Step(); + ++step; } return maxLoops > 0; } @@ -18,3 +20,7 @@ bool WhileTopState(SM &sm, ProgressCode state, uint32_t maxLoops = 5000) { return WhileCondition( sm, [&](int) { return sm.TopLevelState() == state; }, maxLoops); } + +extern void EnsureActiveSlotIndex(uint8_t slot); + +extern void SetFINDAStateAndDebounce(bool press); diff --git a/tests/unit/logic/unload_filament/test_unload_filament.cpp b/tests/unit/logic/unload_filament/test_unload_filament.cpp index ecd7a78..a83e47e 100644 --- a/tests/unit/logic/unload_filament/test_unload_filament.cpp +++ b/tests/unit/logic/unload_filament/test_unload_filament.cpp @@ -27,45 +27,48 @@ namespace mb = modules::buttons; namespace mg = modules::globals; namespace ms = modules::selector; -void RegularUnloadFromSlot04(uint8_t slot) { +bool VerifyState(logic::UnloadFilament &uf, bool filamentLoaded, uint8_t idlerSlotIndex, uint8_t selectorSlotIndex, + bool findaPressed, ml::Mode greenLEDMode, ml::Mode redLEDMode, ErrorCode err, ProgressCode topLevelProgress) { + CHECKED_ELSE(mg::globals.FilamentLoaded() == filamentLoaded) { return false; } + CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex)) { return false; } + CHECKED_ELSE(mi::idler.Engaged() == (idlerSlotIndex < 5)) { return false; } + CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex)) { return false; } + CHECKED_ELSE(ms::selector.Slot() == selectorSlotIndex) { return false; } + CHECKED_ELSE(mf::finda.Pressed() == findaPressed) { return false; } + CHECKED_ELSE(ml::leds.Mode(selectorSlotIndex, ml::red) == redLEDMode) { return false; } + CHECKED_ELSE(ml::leds.Mode(selectorSlotIndex, ml::green) == greenLEDMode) { return false; } + CHECKED_ELSE(uf.Error() == err) { return false; } + CHECKED_ELSE(uf.TopLevelState() == topLevelProgress) { return false; } + return true; +} + +void RegularUnloadFromSlot04Init(uint8_t slot, logic::UnloadFilament &uf) { // prepare startup conditions ForceReinitAllAutomata(); // change the startup to what we need here - // move selector to the right spot - ms::selector.MoveToSlot(slot); - while (ms::selector.Slot() != slot) - main_loop(); + EnsureActiveSlotIndex(slot); - mg::globals.SetActiveSlot(slot); mg::globals.SetFilamentLoaded(true); // set FINDA ON + debounce - hal::adc::SetADC(1, mf::FINDA::adcDecisionLevel + 1); - for (size_t i = 0; i < mf::FINDA::debounce + 1; ++i) - main_loop(); + SetFINDAStateAndDebounce(true); // verify startup conditions - REQUIRE(mg::globals.FilamentLoaded() == true); - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); // restart the automaton - logic::UnloadFilament uf; uf.Reset(slot); +} +void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) { // Stage 0 - verify state just after Reset() - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been activated by the underlying automaton - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); // FINDA triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::blink0); // green LED should blink - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // we still think we have filament loaded at this stage + // idler should have been activated by the underlying automaton + // no change in selector's position + // FINDA on + // green LED should blink, red off + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::UnloadingToFinda)); // run the automaton // Stage 1 - unloading to FINDA @@ -78,53 +81,42 @@ void RegularUnloadFromSlot04(uint8_t slot) { return uf.TopLevelState() == ProgressCode::UnloadingToFinda; }, 5000)); - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(slot)); // idler should have been activated by the underlying automaton - REQUIRE(mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == false); // FINDA triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::blink0); // green LED should blink - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // we still think we have filament loaded at this stage + // idler should have been activated by the underlying automaton + // no change in selector's position + // FINDA triggered off + // green LED should blink + REQUIRE(VerifyState(uf, true, slot, slot, false, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::DisengagingIdler)); // Stage 2 - idler was engaged, disengage it - REQUIRE(uf.TopLevelState() == ProgressCode::DisengagingIdler); REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, 5000)); - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been disengaged - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == false); // FINDA still triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::blink0); // green LED should blink - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // we still think we have filament loaded at this stage + // idler should have been disengaged + // no change in selector's position + // FINDA still triggered off + // green LED should blink + REQUIRE(VerifyState(uf, true, 5, slot, false, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::AvoidingGrind)); // Stage 3 - avoiding grind (whatever is that @@TODO) - REQUIRE(uf.TopLevelState() == ProgressCode::AvoidingGrind); REQUIRE(WhileTopState(uf, ProgressCode::AvoidingGrind, 5000)); - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been disengaged - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == false); // FINDA still triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::blink0); // green LED should blink - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // we still think we have filament loaded at this stage + // idler should have been disengaged + // no change in selector's position + // FINDA still triggered off + // green LED should blink + REQUIRE(VerifyState(uf, true, 5, slot, false, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::FinishingMoves)); // Stage 4 - finishing moves and setting global state correctly - REQUIRE(uf.TopLevelState() == ProgressCode::FinishingMoves); REQUIRE(WhileTopState(uf, ProgressCode::FinishingMoves, 5000)); - REQUIRE(mg::globals.FilamentLoaded() == false); // filament unloaded - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been disengaged - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == false); // FINDA still triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::on); // green LED should be ON - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // filament unloaded + // idler should have been disengaged + // no change in selector's position + // FINDA still triggered off + // green LED should be ON + REQUIRE(VerifyState(uf, false, 5, slot, false, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK)); // Stage 5 - repeated calls to TopLevelState should return "OK" REQUIRE(uf.TopLevelState() == ProgressCode::OK); @@ -135,90 +127,176 @@ void RegularUnloadFromSlot04(uint8_t slot) { TEST_CASE("unload_filament::regular_unload_from_slot_0-4", "[unload_filament]") { for (uint8_t slot = 0; slot < 5; ++slot) { - RegularUnloadFromSlot04(slot); + logic::UnloadFilament uf; + RegularUnloadFromSlot04Init(slot, uf); + RegularUnloadFromSlot04(slot, uf); } } -void FindaDidntTrigger(uint8_t slot) { +void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) { // prepare startup conditions ForceReinitAllAutomata(); // change the startup to what we need here - // move selector to the right spot - ms::selector.MoveToSlot(slot); - while (ms::selector.Slot() != slot) - main_loop(); + EnsureActiveSlotIndex(slot); // set FINDA ON + debounce - hal::adc::SetADC(1, mf::FINDA::adcDecisionLevel + 1); - for (size_t i = 0; i < mf::FINDA::debounce + 1; ++i) - main_loop(); + SetFINDAStateAndDebounce(true); - mg::globals.SetActiveSlot(slot); mg::globals.SetFilamentLoaded(true); // verify startup conditions - REQUIRE(mg::globals.FilamentLoaded() == true); - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); // restart the automaton - logic::UnloadFilament uf; uf.Reset(slot); // Stage 0 - verify state just after Reset() - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been activated by the underlying automaton - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); // FINDA triggered off - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::blink0); // green LED should blink - REQUIRE(uf.Error() == ErrorCode::OK); // no error so far + // we still think we have filament loaded at this stage + // idler should have been activated by the underlying automaton + // no change in selector's position + // FINDA triggered off + // green LED should blink + // no error so far + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::UnloadingToFinda)); // 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, 50000)); - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(slot)); // idler should have been activated by the underlying automaton - REQUIRE(mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); // FINDA still on - REQUIRE(ml::leds.Mode(slot, ml::red) == ml::blink0); // red LED should blink - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::off); // green LED should be off - REQUIRE(uf.Error() == ErrorCode::FINDA_DIDNT_TRIGGER); // didn't get any response from FINDA - REQUIRE(uf.TopLevelState() == ProgressCode::ERR1DisengagingIdler); + // we still think we have filament loaded at this stage + // idler should have been activated by the underlying automaton + // no change in selector's position + // FINDA still on + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, slot, slot, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_TRIGGER, ProgressCode::ERR1DisengagingIdler)); // Stage 2 - idler should get disengaged REQUIRE(WhileTopState(uf, ProgressCode::ERR1DisengagingIdler, 5000)); - REQUIRE(mg::globals.FilamentLoaded() == true); // we still think we have filament loaded at this stage - REQUIRE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // idler should have been disengaged - REQUIRE(!mi::idler.Engaged()); - REQUIRE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot)); // no change in selector's position - REQUIRE(ms::selector.Slot() == slot); - REQUIRE(mf::finda.Pressed() == true); // FINDA still on - REQUIRE(ml::leds.Mode(slot, ml::red) == ml::blink0); // red LED should blink - REQUIRE(ml::leds.Mode(slot, ml::green) == ml::off); // green LED should be off - REQUIRE(uf.Error() == ErrorCode::FINDA_DIDNT_TRIGGER); - REQUIRE(uf.TopLevelState() == ProgressCode::ERR1WaitingForUser); + // we still think we have filament loaded at this stage + // idler should have been disengaged + // no change in selector's position + // FINDA still on + // red LED should blink + // green LED should be off + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_TRIGGER, ProgressCode::ERR1WaitingForUser)); +} + +void FindaDidntTriggerResolveHelp(uint8_t slot, logic::UnloadFilament &uf) { // Stage 3 - the user has to do something // there are 3 options: // - help the filament a bit // - try again the whole sequence // - resolve the problem by hand - after pressing the button we shall check, that FINDA is off and we should do what? + + // In this case we check the first option + + // Perform press on button 1 + debounce + hal::adc::SetADC(0, 0); + while (!mb::buttons.ButtonPressed(0)) { + main_loop(); + uf.Step(); + } + + // we still think we have filament loaded at this stage + // idler should have been disengaged + // no change in selector's position + // FINDA still on + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_TRIGGER, ProgressCode::ERR1EngagingIdler)); + + // Stage 4 - engage the idler + REQUIRE(WhileTopState(uf, ProgressCode::ERR1EngagingIdler, 5000)); + + // we still think we have filament loaded at this stage + // idler should be engaged + // no change in selector's position + // FINDA still on + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, slot, slot, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_TRIGGER, ProgressCode::ERR1HelpingFilament)); } -TEST_CASE("unload_filament::finda_didnt_trigger", "[unload_filament]") { +void FindaDidntTriggerResolveHelpFindaTriggered(uint8_t slot, logic::UnloadFilament &uf) { + // Stage 5 - move the pulley a bit - simulate FINDA depress + REQUIRE(WhileCondition( + uf, + [&](int step) -> bool { + if(step == 100){ // on 100th step make FINDA trigger + hal::adc::SetADC(1, 0); + } + return uf.TopLevelState() == ProgressCode::ERR1HelpingFilament; }, + 5000)); + + // we still think we have filament loaded at this stage + // idler should be engaged + // no change in selector's position + // FINDA depressed + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, slot, slot, false, ml::off, ml::blink0, ErrorCode::OK, ProgressCode::DisengagingIdler)); +} + +void FindaDidntTriggerResolveHelpFindaDidntTrigger(uint8_t slot, logic::UnloadFilament &uf) { + // Stage 5 - move the pulley a bit - no FINDA change + REQUIRE(WhileTopState(uf, ProgressCode::ERR1HelpingFilament, 5000)); + + // we still think we have filament loaded at this stage + // idler should be engaged + // no change in selector's position + // FINDA still pressed + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, slot, slot, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_TRIGGER, ProgressCode::ERR1DisengagingIdler)); +} + +TEST_CASE("unload_filament::finda_didnt_trigger_resolve_help", "[unload_filament]") { for (uint8_t slot = 0; slot < 5; ++slot) { - FindaDidntTrigger(slot); + logic::UnloadFilament uf; + FindaDidntTriggerCommonSetup(slot, uf); + FindaDidntTriggerResolveHelp(slot, uf); + FindaDidntTriggerResolveHelpFindaTriggered(slot, uf); + } + + // the same with different end scenario + for (uint8_t slot = 0; slot < 5; ++slot) { + logic::UnloadFilament uf; + FindaDidntTriggerCommonSetup(slot, uf); + FindaDidntTriggerResolveHelp(slot, uf); + FindaDidntTriggerResolveHelpFindaDidntTrigger(slot, uf); + } +} + +void FindaDidntTriggerResolveTryAgain(uint8_t slot, logic::UnloadFilament &uf) { + // Stage 3 - the user has to do something + // there are 3 options: + // - help the filament a bit + // - try again the whole sequence + // - resolve the problem by hand - after pressing the button we shall check, that FINDA is off and we should do what? + + // In this case we check the second option + + // Perform press on button 2 + debounce + hal::adc::SetADC(0, 340); + while (!mb::buttons.ButtonPressed(1)) { + main_loop(); + uf.Step(); + } + + // we still think we have filament loaded at this stage + // idler should have been disengaged + // no change in selector's position + // FINDA still on + // red LED should blink, green LED should be off + REQUIRE(VerifyState(uf, true, 5, slot, true, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::UnloadingToFinda)); +} + +TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_filament]") { + for (uint8_t slot = 0; slot < 5; ++slot) { + logic::UnloadFilament uf; + FindaDidntTriggerCommonSetup(slot, uf); + FindaDidntTriggerResolveTryAgain(slot, uf); + RegularUnloadFromSlot04(slot, uf); } }