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