From 6973dbff131d5447f0aaded6725227e45373199e Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Fri, 19 Nov 2021 10:39:44 +0100 Subject: [PATCH] Introduce intelligent homing if Idler and Selector Both movable components now perform homing sequences transparently whenever the logic layer invalidates the homingValid flag. That reflects the fact, that the user may have moved the Idler or Selector while trying to resolve a HW issue with un/loading filament. Basic rules: - Idler gets rehomed immediately and then moves into the target slot position - Selector rehomes once it is possible - i.e. when filament load state is AtPulley - then it immediately and spontanneously executes the homing sequence and then returns to the desired state Motivation: - resolve startup issues (EEPROM says we have filament, but FINDA is not triggered) - resolve accidental moves of Idler and/or Selector while digging out stuck filament from the unit --- src/logic/command_base.cpp | 27 ++++++++++++ src/logic/command_base.h | 7 ++++ src/logic/load_filament.cpp | 8 +++- src/logic/tool_change.cpp | 3 ++ src/logic/unload_filament.cpp | 4 ++ src/main.cpp | 11 +++-- src/modules/finda.cpp | 9 +++- src/modules/finda.h | 4 ++ src/modules/idler.cpp | 41 ++++++++++++++----- src/modules/idler.h | 4 -- src/modules/movable_base.cpp | 2 + src/modules/movable_base.h | 22 ++++++++-- src/modules/selector.cpp | 39 +++++++++++++----- src/modules/selector.h | 4 -- .../load_filament/test_load_filament.cpp | 26 ++++++++++-- tests/unit/logic/stubs/main_loop_stub.cpp | 40 +++++++++++++++++- tests/unit/logic/stubs/main_loop_stub.h | 8 ++-- 17 files changed, 213 insertions(+), 46 deletions(-) diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp index a3afe1a..8075b8d 100644 --- a/src/logic/command_base.cpp +++ b/src/logic/command_base.cpp @@ -1,6 +1,8 @@ /// @file command_base.cpp #include "command_base.h" #include "../modules/globals.h" +#include "../modules/finda.h" +#include "../modules/fsensor.h" #include "../modules/idler.h" #include "../modules/selector.h" #include "../modules/motion.h" @@ -89,6 +91,31 @@ void CommandBase::Panic(ErrorCode ec) { } } +void CommandBase::InvalidateHoming() { + mi::idler.InvalidateHoming(); + ms::selector.InvalidateHoming(); +} + +void CommandBase::InvalidateHomingAndFilamentState() { + InvalidateHoming(); + + // reset the filament presence according to available sensor states + bool fs = mfs::fsensor.Pressed(); + bool fi = mf::finda.Pressed(); + + if (fs && fi) { + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::InNozzle); + } else if (!fs && fi) { + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::InSelector); + } else if (!fs && !fi) { + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::AtPulley); + } else { + // we can't say for sure - definitely an error in sensors or something is blocking them + // let's assume there is a piece of filament present in the selector + mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::InSelector); + } +} + bool CommandBase::CheckToolIndex(uint8_t index) { if (index >= config::toolCount) { error = ErrorCode::INVALID_TOOL; diff --git a/src/logic/command_base.h b/src/logic/command_base.h index 641180a..bd5d75b 100644 --- a/src/logic/command_base.h +++ b/src/logic/command_base.h @@ -70,6 +70,13 @@ public: /// The only way out is to reset the board. void Panic(ErrorCode ec); + /// Invalidates homing state on Idler and Selector - doesn't change anything about filament load status + static void InvalidateHoming(); + + /// Invalidates homing state on Idler and Selector + resets the knowledge about + /// filament presence according to known sensors (FINDA+FSensor) + static void InvalidateHomingAndFilamentState(); + protected: /// @returns true if the slot/tool index is within specified range (0 - config::toolCount) /// If not, it returns false and sets the error to ErrorCode::INVALID_TOOL diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index 82cb5bc..a3ca344 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -71,7 +71,10 @@ bool LoadFilament::StepInner() { } break; case ProgressCode::DisengagingIdler: - if (!mi::idler.Engaged()) { + // beware - this state is being reused for error recovery + // and if the selector decided to re-home, we have to wait for it as well + // therefore: 'if (!mi::idler.Engaged())' : alone is not enough + if (!mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) { FinishedCorrectly(); } break; @@ -97,6 +100,9 @@ bool LoadFilament::StepInner() { break; case mui::Event::Right: // problem resolved - the user pushed the fillament by hand? // we should check the state of all the sensors and either report another error or confirm the correct state + + // First invalidate homing flags as the user may have moved the Idler or Selector accidentally + InvalidateHoming(); if (!mf::finda.Pressed()) { // FINDA is still NOT pressed - that smells bad error = ErrorCode::FINDA_DIDNT_SWITCH_ON; diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp index 3e8279a..cc5a854 100644 --- a/src/logic/tool_change.cpp +++ b/src/logic/tool_change.cpp @@ -108,6 +108,9 @@ bool ToolChange::StepInner() { break; case mui::Event::Right: // problem resolved - the user pushed the fillament by hand? // we should check the state of all the sensors and either report another error or confirm the correct state + + // First invalidate homing flags as the user may have moved the Idler or Selector accidentally + InvalidateHoming(); if (!mf::finda.Pressed()) { // FINDA is still NOT pressed - that smells bad error = ErrorCode::FINDA_DIDNT_SWITCH_ON; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index c0dc926..a80e421 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -7,6 +7,7 @@ #include "../modules/leds.h" #include "../modules/motion.h" #include "../modules/permanent_storage.h" +#include "../modules/selector.h" #include "../modules/user_input.h" #include "../debug.h" @@ -93,6 +94,9 @@ bool UnloadFilament::StepInner() { break; case mui::Event::Right: // problem resolved - the user pulled the fillament by hand // we should check the state of all the sensors and either report another error or confirm the correct state + + // First invalidate homing flags as the user may have moved the Idler or Selector accidentally + InvalidateHoming(); if (mfs::fsensor.Pressed()) { // printer's filament sensor is still pressed - that smells bad error = ErrorCode::FSENSOR_DIDNT_SWITCH_OFF; diff --git a/src/main.cpp b/src/main.cpp index 9739e6f..ec79219 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -102,10 +102,9 @@ void setup() { mu::cdc.Init(); - ms::selector.Init(); // selector decides if homing is possible - mi::idler.Home(); // home Idler every time - - _delay_ms(100); + // waits at least finda debounce period + // which is abused to let the LEDs shine for ~100ms + mf::finda.BlockingInit(); /// Turn off all leds for (uint8_t i = 0; i < config::toolCount; i++) { @@ -113,6 +112,10 @@ void setup() { ml::leds.SetMode(i, ml::red, ml::off); } ml::leds.Step(); + + // Idler will home on its own by default + // Selector decides whether homing is possible + ms::selector.Init(); } static constexpr const uint8_t maxMsgLen = 10; diff --git a/src/modules/finda.cpp b/src/modules/finda.cpp index 4f3efb1..41b46d6 100644 --- a/src/modules/finda.cpp +++ b/src/modules/finda.cpp @@ -10,9 +10,16 @@ namespace finda { FINDA finda; void FINDA::Step() { - debounce::Debouncer::Step(mt::timebase.Millis(), hal::gpio::ReadPin(FINDA_PIN) == hal::gpio::Level::high); } +void FINDA::BlockingInit() { + auto tgtMs = mt::timebase.Millis() + config::findaDebounceMs + 1; + Step(); // let FINDA settle down - we're gonna need its state for selector homing + while (tgtMs < mt::timebase.Millis()) { + mf::finda.Step(); + } +} + } // namespace finda } // namespace modules diff --git a/src/modules/finda.h b/src/modules/finda.h index 2351e87..f536a76 100644 --- a/src/modules/finda.h +++ b/src/modules/finda.h @@ -18,6 +18,10 @@ public: /// Performs one step of the state machine - reads the digital pin, processes debouncing, updates states of FINDA void Step(); + /// Initialize the FINDA - waits at least config::findaDebounceMs + /// to set correct FINDA state at startup + void BlockingInit(); + using debounce::Debouncer::Pressed; }; diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 10ba004..3f757a7 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -23,8 +23,11 @@ void Idler::PlanHomingMove() { void Idler::FinishHomingAndPlanMoveToParkPos() { mm::motion.SetPosition(mm::Idler, 0); - plannedSlot = IdleSlotIndex(); - plannedEngage = false; + + // finish whatever has been planned before homing + if (!plannedEngage) { + plannedSlot = IdleSlotIndex(); + } InitMovement(mm::Idler); } @@ -42,10 +45,19 @@ Idler::OperationResult Idler::Disengage() { plannedSlot = IdleSlotIndex(); plannedEngage = false; + // coordinates invalid, first home, then disengage + if (!homingValid) { + PerformHome(mm::Idler); + return OperationResult::Accepted; + } + + // already disengaged if (!Engaged()) { dbg_logic_P(PSTR("Idler Disengaged")); return OperationResult::Accepted; } + + // disengaging return InitMovement(mm::Idler); } @@ -58,22 +70,27 @@ Idler::OperationResult Idler::Engage(uint8_t slot) { plannedSlot = slot; plannedEngage = true; + // if we are homing right now, just record the desired planned slot and return Accepted + if (state == Homing) { + return OperationResult::Accepted; + } + + // coordinates invalid, first home, then engage + if (!homingValid) { + PlanHome(mm::Idler); + return OperationResult::Accepted; + } + + // already engaged if (Engaged()) { dbg_logic_P(PSTR("Idler Engaged")); return OperationResult::Accepted; } + // engaging return InitMovement(mm::Idler); } -bool Idler::Home() { - if (state == Moving) - return false; - plannedEngage = false; - PlanHome(mm::Idler); - return true; -} - bool Idler::Step() { switch (state) { case Moving: @@ -85,6 +102,10 @@ bool Idler::Step() { PerformHome(mm::Idler); return false; case Ready: + if (!homingValid) { + PlanHome(mm::Idler); + return false; + } return true; case Failed: dbg_logic_P(PSTR("Idler Failed")); diff --git a/src/modules/idler.h b/src/modules/idler.h index 7ed82e6..bdae406 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -28,10 +28,6 @@ public: /// @returns #OperationResult OperationResult Disengage(); - /// Plan homing of the idler axis - /// @returns false in case an operation is already underway - bool Home(); - /// Performs one step of the state machine according to currently planned operation /// @returns true if the idler is ready to accept new commands (i.e. it has finished the last operation) bool Step(); diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 5f4deb8..b367ade 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -46,10 +46,12 @@ void MovableBase::PerformHome(config::Axis axis) { // we have reached the end of the axis - homed ok mm::motion.AbortPlannedMoves(axis, true); mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal); + homingValid = true; FinishHomingAndPlanMoveToParkPos(); // state = Ready; // not yet - we have to move to our parking position after homing the axis } else if (mm::motion.QueueEmpty(axis)) { // we ran out of planned moves but no StallGuard event has occurred - homing failed + homingValid = false; mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal); state = Failed; } diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 44c0c3a..b765aeb 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -28,7 +28,8 @@ public: inline constexpr MovableBase() : state(Ready) , plannedSlot(-1) - , currentSlot(-1) {} + , currentSlot(-1) + , homingValid(false) {} /// virtual ~MovableBase(); intentionally disabled, see description in logic::CommandBase @@ -42,8 +43,17 @@ public: inline hal::tmc2130::ErrorFlags TMCErrorFlags() const { return tmcErrorFlags; } - /// Prepare a homing move of the axis - void PlanHome(config::Axis axis); + /// Invalidates the homing flag - that is now used to inform the movable component (Idler or Selector) + /// that their current coordinates may have been compromised and a new homing move is to be performed. + /// Each movable component performs the homing move immediately after it is possible to do so: + /// - Idler immediately (and then moves to desired slot again) + /// - Selector once there is no filament stuck in it (and then moves to desired slot again) + /// Homing procedure therefore becomes completely transparent to upper layers + /// and it will not be necessary to call it explicitly. + /// Please note this method does not clear any planned move on the component + /// - on the contrary - the planned move will be peformed immediately after homing + /// (which makes homing completely transparent) + inline void InvalidateHoming() { homingValid = false; } protected: /// internal state of the automaton @@ -55,6 +65,9 @@ protected: /// current slot uint8_t currentSlot; + /// true if the axis is considered as homed + bool homingValid; + /// cached TMC2130 error flags - being read only if the axis is enabled and doing something (moving) hal::tmc2130::ErrorFlags tmcErrorFlags; @@ -65,6 +78,9 @@ protected: OperationResult InitMovement(config::Axis axis); + /// Prepare a homing move of the axis + void PlanHome(config::Axis axis); + void PerformMove(config::Axis axis); void PerformHome(config::Axis axis); diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index a2dddc4..6f2e4f1 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -1,6 +1,7 @@ /// @file selector.cpp #include "selector.h" #include "buttons.h" +#include "finda.h" #include "leds.h" #include "motion.h" #include "permanent_storage.h" @@ -24,7 +25,12 @@ void Selector::PlanHomingMove() { void Selector::FinishHomingAndPlanMoveToParkPos() { mm::motion.SetPosition(mm::Selector, mm::unitToSteps(config::selectorLimits.lenght)); - plannedSlot = IdleSlotIndex(); + currentSlot = -1; + + // finish whatever has been planned before homing + if (plannedSlot > config::toolCount) { + plannedSlot = IdleSlotIndex(); + } InitMovement(mm::Selector); } @@ -39,18 +45,25 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) { } plannedSlot = slot; + // if we are homing right now, just record the desired planned slot and return Accepted + if (state == Homing) { + return OperationResult::Accepted; + } + + // coordinates invalid, first home, then engage + if (!homingValid && mg::globals.FilamentLoaded() < mg::FilamentLoadState::InSelector) { + PlanHome(mm::Selector); + return OperationResult::Accepted; + } + + // already at the right slot if (currentSlot == slot) { dbg_logic_P(PSTR("Moving Selector")); return OperationResult::Accepted; } - return InitMovement(mm::Selector); -} -bool Selector::Home() { - if (state == Moving) - return false; - PlanHome(mm::Selector); - return true; + // do the move + return InitMovement(mm::Selector); } bool Selector::Step() { @@ -64,7 +77,10 @@ bool Selector::Step() { PerformHome(mm::Selector); return false; case Ready: - //dbg_logic_P(PSTR("Selector Ready")); + if (!homingValid && mg::globals.FilamentLoaded() < mg::InSelector) { + PlanHome(mm::Selector); + return false; + } return true; case Failed: dbg_logic_P(PSTR("Selector Failed")); @@ -74,12 +90,13 @@ bool Selector::Step() { } void Selector::Init() { - if (mg::globals.FilamentLoaded() < mg::FilamentLoadState::InSelector) { + if (mg::globals.FilamentLoaded() < mg::FilamentLoadState::InSelector && (!mf::finda.Pressed())) { // home the Selector only in case we don't have filament loaded (or at least we think we don't) - Home(); + PlanHome(mm::Selector); } else { // otherwise set selector's position according to know slot positions (and pretend it is correct) mm::motion.SetPosition(mm::Selector, SlotPosition(mg::globals.ActiveSlot()).v); + InvalidateHoming(); // and plan homing sequence ASAP } } diff --git a/src/modules/selector.h b/src/modules/selector.h index 113b276..f2fcec1 100644 --- a/src/modules/selector.h +++ b/src/modules/selector.h @@ -22,10 +22,6 @@ public: /// @returns false in case an operation is already underway OperationResult MoveToSlot(uint8_t slot); - /// Plan homing of the selector's axis - /// @returns false in case an operation is already underway - bool Home(); - /// Performs one step of the state machine according to currently planned operation. /// @returns true if the selector is ready to accept new commands (i.e. it has finished the last operation) bool Step(); diff --git a/tests/unit/logic/load_filament/test_load_filament.cpp b/tests/unit/logic/load_filament/test_load_filament.cpp index 958d5f3..939abf9 100644 --- a/tests/unit/logic/load_filament/test_load_filament.cpp +++ b/tests/unit/logic/load_filament/test_load_filament.cpp @@ -137,8 +137,11 @@ void FailedLoadToFindaResolveManual(uint8_t slot, logic::LoadFilament &lf) { hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high); PressButtonAndDebounce(lf, mb::Right); + // the Idler also engages in this call as this is planned as the next step + SimulateIdlerHoming(); + // pulling filament back - REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda)); + REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, true, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda)); // Stage 3 - retracting from finda // we'll assume the finda is working correctly here @@ -150,9 +153,24 @@ void FailedLoadToFindaResolveManual(uint8_t slot, logic::LoadFilament &lf) { } return lf.TopLevelState() == ProgressCode::RetractingFromFinda; }, 5000)); - REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler)); - // disengaging idler + // This is a tricky part as the Selector will start homing asynchronnously right after + // the filament state switches to AtPulley. + // The trouble is, that the filament state is updated after the Pulley finishes + // its moves (which is correct), but we don't have enough cycles to home the selector afterwards + // - basically it will just start homing + // Moreover, the Idler is to disengage meanwhile, which makes the simulation even harder. + // Therefore we just tick the stallguard of the Selector and hope for the best + mm::TriggerStallGuard(mm::Selector); + ms::selector.Step(); + mm::motion.StallGuardReset(mm::Selector); // drop stallguard on Selector to avoid future confusion + + // just one step is necessary to "finish" homing + // but the selector then (correctly) plans its move to the original position + // therefore we expect the selector to have its idle position at this stage + // REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, ms::selector.IdleSlotIndex(), false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler)); + + // disengaging idler (and the selector will move to the desired position meanwhile REQUIRE(WhileTopState(lf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps)); REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); } @@ -161,6 +179,8 @@ void FailedLoadToFindaResolveManualNoFINDA(uint8_t slot, logic::LoadFilament &lf // Perform press on button 2 + debounce + keep FINDA OFF (i.e. the user didn't solve anything) PressButtonAndDebounce(lf, mb::Right); + SimulateIdlerHoming(); + // pulling filament back REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRWaitingForUser)); } diff --git a/tests/unit/logic/stubs/main_loop_stub.cpp b/tests/unit/logic/stubs/main_loop_stub.cpp index 848ca17..2ba39e1 100644 --- a/tests/unit/logic/stubs/main_loop_stub.cpp +++ b/tests/unit/logic/stubs/main_loop_stub.cpp @@ -72,9 +72,12 @@ void ForceReinitAllAutomata() { } void HomeIdlerAndSelector() { - ms::selector.Home(); - mi::idler.Home(); + ms::selector.InvalidateHoming(); + mi::idler.InvalidateHoming(); + SimulateIdlerAndSelectorHoming(); +} +void SimulateIdlerAndSelectorHoming() { // do 5 steps until we trigger the simulated stallguard for (uint8_t i = 0; i < 5; ++i) { main_loop(); @@ -86,6 +89,39 @@ void HomeIdlerAndSelector() { // now the Selector and Idler shall perform a move into their parking positions while (ms::selector.State() != mm::MovableBase::Ready || mi::idler.State() != mm::MovableBase::Ready) main_loop(); + + mm::motion.StallGuardReset(mm::Selector); + mm::motion.StallGuardReset(mm::Idler); +} + +void SimulateIdlerHoming() { + // do 5 steps until we trigger the simulated stallguard + for (uint8_t i = 0; i < 5; ++i) { + main_loop(); + } + + mm::TriggerStallGuard(mm::Idler); + + // now the Idler shall perform a move into their parking positions + while (mi::idler.State() != mm::MovableBase::Ready) + main_loop(); + + mm::motion.StallGuardReset(mm::Idler); +} + +void SimulateSelectorHoming() { + // do 5 steps until we trigger the simulated stallguard + for (uint8_t i = 0; i < 5; ++i) { + main_loop(); + } + + mm::TriggerStallGuard(mm::Selector); + + // now the Selector shall perform a move into their parking positions + while (ms::selector.State() != mm::MovableBase::Ready) + main_loop(); + + mm::motion.StallGuardReset(mm::Selector); } void EnsureActiveSlotIndex(uint8_t slot, mg::FilamentLoadState loadState) { diff --git a/tests/unit/logic/stubs/main_loop_stub.h b/tests/unit/logic/stubs/main_loop_stub.h index 0e2a5a0..7c8c3ac 100644 --- a/tests/unit/logic/stubs/main_loop_stub.h +++ b/tests/unit/logic/stubs/main_loop_stub.h @@ -22,9 +22,11 @@ bool WhileTopState(SM &sm, ProgressCode state, uint32_t maxLoops = 5000) { sm, [&](uint32_t) { return sm.TopLevelState() == state; }, maxLoops); } -extern void EnsureActiveSlotIndex(uint8_t slot, modules::globals::FilamentLoadState loadState); - -extern void SetFINDAStateAndDebounce(bool press); +void EnsureActiveSlotIndex(uint8_t slot, modules::globals::FilamentLoadState loadState); +void SetFINDAStateAndDebounce(bool press); +void SimulateIdlerHoming(); +void SimulateSelectorHoming(); +void SimulateIdlerAndSelectorHoming(); // 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