diff --git a/src/config/config.h b/src/config/config.h index fad203f..cf73064 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -204,6 +204,16 @@ static constexpr U_deg idlerSlotPositions[toolCount + 1] = { IdlerOffsetFromHome ///18.0_deg Fully disengaged all slots }; +/// Intermediate positions for Idler's slots: 0-4 are the real ones, the 5th index is the idle position +static constexpr U_deg idlerIntermediateSlotPositions[toolCount + 1] = { + IdlerOffsetFromHome + 4.5F * IdlerSlotDistance, + IdlerOffsetFromHome + 3.5F * IdlerSlotDistance, + IdlerOffsetFromHome + 2.5F * IdlerSlotDistance, + IdlerOffsetFromHome + 1.5F * IdlerSlotDistance, + IdlerOffsetFromHome + 0.5F * IdlerSlotDistance, + IdlerOffsetFromHome ///18.0_deg Fully disengaged all slots +}; + static constexpr U_deg idlerParkPositionDelta = -IdlerSlotDistance + 5.0_deg / 2; ///@TODO verify static constexpr U_deg_s idlerFeedrate = 300._deg_s; diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp index f870f50..8cf4ccf 100644 --- a/src/logic/command_base.cpp +++ b/src/logic/command_base.cpp @@ -187,7 +187,7 @@ bool CommandBase::CheckToolIndex(uint8_t index) { } void CommandBase::ErrDisengagingIdler() { - if (!mi::idler.Engaged()) { + if (mi::idler.Disengaged()) { state = ProgressCode::ERRWaitingForUser; error = deferredErrorCode; deferredErrorCode = ErrorCode::OK; // and clear the deferredEC just for safety diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp index 19f543f..223eacb 100644 --- a/src/logic/eject_filament.cpp +++ b/src/logic/eject_filament.cpp @@ -66,7 +66,7 @@ bool EjectFilament::StepInner() { } break; case ProgressCode::DisengagingIdler: - if (!mi::idler.Engaged()) { // idler disengaged + if (mi::idler.Disengaged()) { // idler disengaged mpu::pulley.Disable(); mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::NotLoaded); FinishedOK(); diff --git a/src/logic/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp index 7f71b79..8d6d1eb 100644 --- a/src/logic/feed_to_bondtech.cpp +++ b/src/logic/feed_to_bondtech.cpp @@ -69,18 +69,25 @@ bool FeedToBondtech::Step() { case PushingFilamentIntoNozzle: if (mm::motion.QueueEmpty()) { mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle); - mi::idler.Disengage(); + mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // while disengaging the idler, keep on moving with the pulley to avoid grinding while the printer is trying to grab the filament itself mpu::pulley.PlanMove(config::fsensorToNozzleAvoidGrind, config::pulleySlowFeedrate); + state = PartiallyDisengagingIdler; + } + return false; + case PartiallyDisengagingIdler: + if (mi::idler.PartiallyDisengaged()) { + mm::motion.AbortPlannedMoves(mm::Pulley); + mpu::pulley.Disable(); + mi::idler.Disengage(); // disengage fully while Pulley is already stopped state = DisengagingIdler; } return false; case DisengagingIdler: - if (!mi::idler.Engaged()) { + if (mi::idler.Disengaged()) { dbg_logic_P(PSTR("Feed to Bondtech --> Idler disengaged")); dbg_logic_fP(PSTR("Pulley end steps %u"), mpu::pulley.CurrentPosition_mm()); state = OK; - mpu::pulley.Disable(); ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on); } return false; diff --git a/src/logic/feed_to_bondtech.h b/src/logic/feed_to_bondtech.h index 146c48a..3f55940 100644 --- a/src/logic/feed_to_bondtech.h +++ b/src/logic/feed_to_bondtech.h @@ -17,6 +17,7 @@ struct FeedToBondtech { PushingFilamentFast, PushingFilamentToFSensor, PushingFilamentIntoNozzle, + PartiallyDisengagingIdler, DisengagingIdler, OK, Failed, diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index 0d11a40..310ed9b 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -100,8 +100,8 @@ bool LoadFilament::StepInner() { case ProgressCode::DisengagingIdler: // 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()) { + // therefore: 'if (mi::idler.Disengaged())' : alone is not enough + if (mi::idler.Disengaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) { LoadFinishedCorrectly(); } break; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 122367f..086fe9d 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -79,7 +79,7 @@ bool UnloadFilament::StepInner() { } return false; case ProgressCode::DisengagingIdler: - if (!mi::idler.Engaged() && ms::selector.State() == ms::Selector::Ready) { + if (mi::idler.Disengaged() && ms::selector.State() == ms::Selector::Ready) { UnloadFinishedCorrectly(); } return false; diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 21c7dfd..29be930 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -13,7 +13,9 @@ namespace idler { Idler idler; void Idler::PrepareMoveToPlannedSlot() { - mm::motion.PlanMoveTo(SlotPosition(plannedSlot), mm::unitToAxisUnit(config::idlerFeedrate)); + mm::motion.PlanMoveTo( + plannedMove == Operation::engage ? SlotPosition(plannedSlot) : IntermediateSlotPosition(plannedSlot), + mm::unitToAxisUnit(config::idlerFeedrate)); dbg_logic_fP(PSTR("Prepare Move Idler slot %d"), plannedSlot); } @@ -40,7 +42,7 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() { mm::motion.SetPosition(mm::Idler, 0); // finish whatever has been planned before homing - if (!plannedEngage) { + if (plannedMove == Operation::disengage) { plannedSlot = IdleSlotIndex(); } InitMovement(); @@ -48,8 +50,8 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() { } void Idler::FinishMove() { - currentlyEngaged = plannedEngage; - if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary) + currentlyEngaged = plannedMove; + if (Disengaged()) // turn off power into the Idler motor when disengaged (no force necessary) mm::motion.Disable(mm::Idler); } @@ -59,7 +61,7 @@ Idler::OperationResult Idler::Disengage() { return OperationResult::Refused; } plannedSlot = IdleSlotIndex(); - plannedEngage = false; + plannedMove = Operation::disengage; // coordinates invalid, first home, then disengage if (!homingValid) { @@ -68,7 +70,7 @@ Idler::OperationResult Idler::Disengage() { } // already disengaged - if (!Engaged()) { + if (Disengaged()) { dbg_logic_P(PSTR("Idler Disengaged")); return OperationResult::Accepted; } @@ -77,14 +79,22 @@ Idler::OperationResult Idler::Disengage() { return InitMovement(); } +Idler::OperationResult Idler::PartiallyDisengage(uint8_t slot) { + return PlanMoveInner(slot, Operation::intermediate); +} + Idler::OperationResult Idler::Engage(uint8_t slot) { + return PlanMoveInner(slot, Operation::engage); +} + +Idler::OperationResult Idler::PlanMoveInner(uint8_t slot, Operation plannedOp) { if (state == Moving) { dbg_logic_P(PSTR("Moving --> Engage refused")); return OperationResult::Refused; } plannedSlot = slot; - plannedEngage = true; + plannedMove = plannedOp; // if we are homing right now, just record the desired planned slot and return Accepted if (state == HomeBack) { @@ -101,12 +111,10 @@ Idler::OperationResult Idler::Engage(uint8_t slot) { } // already engaged - if (Engaged()) { - dbg_logic_P(PSTR("Idler Engaged")); + if (currentlyEngaged == plannedMove) { return OperationResult::Accepted; } - // engaging return InitMovement(); } diff --git a/src/modules/idler.h b/src/modules/idler.h index 17a89b4..c0b6a0c 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -16,8 +16,8 @@ class Idler : public motion::MovableBase { public: inline constexpr Idler() : MovableBase(mm::Idler) - , plannedEngage(false) - , currentlyEngaged(false) {} + , plannedMove(Operation::disengage) + , currentlyEngaged(Operation::disengage) {} /// Plan engaging of the idler to a specific filament slot /// @param slot index to be activated @@ -28,20 +28,31 @@ public: /// @returns #OperationResult OperationResult Disengage(); + /// Plan partial disengaging of the idler + /// @returns #OperationResult + OperationResult PartiallyDisengage(uint8_t slot); + /// 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(); - /// @returns the current state of idler - engaged / disengaged + /// @returns the current state of idler - engaged / disengaged / partially disengaged /// this state is updated only when a planned move is successfully finished, so it is safe for higher-level /// state machines to use this call as a waiting condition for the desired state of the idler - inline bool Engaged() const { return currentlyEngaged; } + inline bool Engaged() const { return currentlyEngaged == Operation::engage; } + inline bool Disengaged() const { return currentlyEngaged == Operation::disengage; } + inline bool PartiallyDisengaged() const { return currentlyEngaged == Operation::intermediate; } /// @returns predefined positions of individual slots static constexpr mm::I_pos_t SlotPosition(uint8_t slot) { return mm::unitToAxisUnit(config::idlerSlotPositions[slot]); } + /// @returns predefined intermediate positions between individual slots + static constexpr mm::I_pos_t IntermediateSlotPosition(uint8_t slot) { + return mm::unitToAxisUnit(config::idlerIntermediateSlotPositions[slot]); + } + /// @returns the index of idle position of the idler, usually 5 in case of 0-4 valid indices of filament slots inline static constexpr uint8_t IdleSlotIndex() { return config::toolCount; } @@ -59,11 +70,19 @@ protected: virtual void FinishMove() override; private: - /// direction of travel - engage/disengage - bool plannedEngage; + enum class Operation : uint8_t { + disengage = 0, + engage = 1, + intermediate = 2 ///< planned movement will be to an intermediate position between slots (different array of positions) + }; + + OperationResult PlanMoveInner(uint8_t slot, Operation plannedOp); + + /// direction of travel - engage/disengage/disengageIntermediate + Operation plannedMove; /// current state - bool currentlyEngaged; + Operation currentlyEngaged; }; /// The one and only instance of Idler in the FW diff --git a/tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp b/tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp index 2053a8a..33103ef 100644 --- a/tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp +++ b/tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp @@ -24,8 +24,10 @@ namespace ha = hal::adc; TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") { using namespace logic; + uint8_t slot = 0; + ForceReinitAllAutomata(); - REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley)); + REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley)); FeedToBondtech fb; main_loop(); @@ -35,10 +37,10 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") { REQUIRE(fb.State() == FeedToBondtech::EngagingIdler); - // it should have instructed the selector and idler to move to slot 0 + // it should have instructed the selector and idler to move to a slot // check if the idler and selector have the right command - CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(0).v); - CHECK(mm::AxisNearestTargetPos(mm::Selector) == ms::Selector::SlotPosition(0).v); + CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(slot).v); + CHECK(mm::AxisNearestTargetPos(mm::Selector) == ms::Selector::SlotPosition(slot).v); CHECK(mm::axes[mm::Idler].enabled == true); // engaging idler @@ -47,8 +49,8 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") { [&](uint32_t) { return !mi::idler.Engaged(); }, 5000)); - CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v); - CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v); + CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(slot).v); + CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v); CHECK(mm::axes[mm::Pulley].enabled); // idler engaged, selector in position, we'll start pushing filament @@ -81,15 +83,26 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") { [&](uint32_t) { return fb.State() == FeedToBondtech::PushingFilamentIntoNozzle; }, 5000)); - // disengaging idler + // partially disengaging idler + REQUIRE(fb.State() == FeedToBondtech::PartiallyDisengagingIdler); + REQUIRE(WhileCondition( + fb, + [&](uint32_t) { return fb.State() == FeedToBondtech::PartiallyDisengagingIdler; }, + 5000)); + + CHECK(mm::axes[mm::Idler].pos == mi::Idler::IntermediateSlotPosition(slot).v); + CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v); + CHECK_FALSE(mm::axes[mm::Pulley].enabled); + + // fully disengaging idler REQUIRE(fb.State() == FeedToBondtech::DisengagingIdler); REQUIRE(WhileCondition( fb, [&](uint32_t) { return fb.State() == FeedToBondtech::DisengagingIdler; }, 5000)); - CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5).v); - CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v); + CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(mi::idler.IdleSlotIndex()).v); + CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v); CHECK_FALSE(mm::axes[mm::Pulley].enabled); // state machine finished ok, the green LED should be on