Introduce Idler::PartiallyDisengage

This is to solve a potential problem while feeding to printer's drive gears - while disengaging the Idler, the Pulley was still rotating to avoid grinding the filament (printer is pulling it).
Other filaments could have moved a bit when the Idler's bearings ran over them while the Pulley was still rotating slowly -> the filament could have been moved into the Selector's path causing trouble (especially when not used in the print).
Therefore, the Idler disengages partially now - moves into an intermediate position between the slots.
Then, the Pulley is completely stopped and after that the Idler does a full disengage like before.

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;

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

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();

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;

src/logic/feed_to_bondtech.h

@@ -17,6 +17,7 @@ struct FeedToBondtech {
         PushingFilamentFast,
         PushingFilamentToFSensor,
         PushingFilamentIntoNozzle,
+        PartiallyDisengagingIdler,
         DisengagingIdler,
         OK,
         Failed,

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;

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;

src/modules/idler.cpp

@@ -13,7 +13,9 @@ namespace idler {
 Idler idler;
 
 void Idler::PrepareMoveToPlannedSlot() {
-    mm::motion.PlanMoveTo<mm::Idler>(SlotPosition(plannedSlot), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
+    mm::motion.PlanMoveTo<mm::Idler>(
+        plannedMove == Operation::engage ? SlotPosition(plannedSlot) : IntermediateSlotPosition(plannedSlot),
+        mm::unitToAxisUnit<mm::I_speed_t>(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();
 }
 

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<mm::I_pos_t>(config::idlerSlotPositions[slot]);
     }
 
+    /// @returns predefined intermediate positions between individual slots
+    static constexpr mm::I_pos_t IntermediateSlotPosition(uint8_t slot) {
+        return mm::unitToAxisUnit<mm::I_pos_t>(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

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