Fix and tune CutFilament + unit tests

Several issues addressed in this PR:
- CutFilament tuning + error recovery
- introduce register 0x1d (cut filament selector iRun current level)
- optimize setting iRun and iHold currents in the FW
- CutFilament unit test fixed

src/config/config.h

@@ -134,6 +134,8 @@ static constexpr AxisConfig selector = {
     .sg_thrs = 3,
 };
 
+static constexpr uint8_t selectorCutIRun = 40; ///< 660mA
+
 /// Selector motion limits
 static constexpr SelectorLimits selectorLimits = {
     .lenght = 75.0_mm, // @@TODO how does this relate to SelectorOffsetFromMin?

src/logic/cut_filament.cpp

@@ -9,6 +9,7 @@
 #include "../modules/permanent_storage.h"
 #include "../modules/pulley.h"
 #include "../modules/selector.h"
+#include "../modules/user_input.h"
 
 namespace logic {
 
@@ -34,10 +35,16 @@ bool CutFilament::Reset(uint8_t param) {
 void CutFilament::SelectFilamentSlot() {
     state = ProgressCode::SelectingFilamentSlot;
     mi::idler.Engage(cutSlot);
-    ms::selector.MoveToSlot(cutSlot);
+    MoveSelector(cutSlot);
     ml::leds.SetPairButOffOthers(cutSlot, ml::blink0, ml::off);
 }
 
+void CutFilament::MoveSelector(uint8_t slot) {
+    if (ms::selector.MoveToSlot(slot) != ms::Selector::OperationResult::Accepted) {
+        GoToErrDisengagingIdler(ErrorCode::FINDA_FLICKERS);
+    }
+}
+
 bool CutFilament::StepInner() {
     switch (state) {
     case ProgressCode::UnloadingFilament:
@@ -51,55 +58,102 @@ bool CutFilament::StepInner() {
     case ProgressCode::SelectingFilamentSlot:
         if (mi::idler.Engaged() && ms::selector.Slot() == cutSlot) { // idler and selector finished their moves
             mg::globals.SetFilamentLoaded(cutSlot, mg::FilamentLoadState::AtPulley);
-            feed.Reset(true, true);
+            if (feed.Reset(true, true)) {
                 state = ProgressCode::FeedingToFinda;
+                error = ErrorCode::RUNNING;
+            } else {
+                // selector refused to move - FINDA problem suspected
+                GoToErrDisengagingIdler(ErrorCode::FINDA_FLICKERS);
+            }
         }
         break;
-    case ProgressCode::FeedingToFinda: // @@TODO this state will be reused for repeated cutting of filament ... probably there will be multiple attempts, not sure
+    case ProgressCode::FeedingToFinda:
         if (feed.Step()) {
             if (feed.State() == FeedToFinda::Failed) {
-                // @@TODO
+                GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON); // signal loading error
             } else {
                 // unload back to the pulley
-                state = ProgressCode::UnloadingToPulley;
+                state = ProgressCode::RetractingFromFinda;
-                mpu::pulley.PlanMove(-config::cutLength, mg::globals.PulleyUnloadFeedrate_mm_s());
+                retract.Reset();
             }
         }
         break;
-    case ProgressCode::UnloadingToPulley:
+    case ProgressCode::RetractingFromFinda:
-        if (mm::motion.QueueEmpty()) { // idler and selector finished their moves
+        if (retract.Step()) {
+            if (retract.State() == RetractFromFinda::Failed) {
+                GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); // signal loading error
+            } else {
                 // move selector aside - prepare the blade into active position
                 state = ProgressCode::PreparingBlade;
                 mg::globals.SetFilamentLoaded(cutSlot, mg::FilamentLoadState::AtPulley);
-            ms::selector.MoveToSlot(cutSlot + 1);
+                ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off);
+                MoveSelector(cutSlot + 1);
             }
+        }
+        break;
     case ProgressCode::PreparingBlade:
         if (ms::selector.Slot() == cutSlot + 1) {
             state = ProgressCode::PushingFilament;
-            mpu::pulley.PlanMove(config::cutLength, mg::globals.PulleyUnloadFeedrate_mm_s()); //
+            mpu::pulley.PlanMove(config::cutLength + config::cuttingEdgeRetract, config::pulleySlowFeedrate);
         }
         break;
     case ProgressCode::PushingFilament:
         if (mm::motion.QueueEmpty()) {
+            state = ProgressCode::DisengagingIdler;
+            mi::idler.Disengage(); // disengage before doing a cut because we need extra power into the Selector motor
+        }
+        break;
+    case ProgressCode::DisengagingIdler:
+        if (mi::idler.Disengaged()) {
             state = ProgressCode::PerformingCut;
-            ms::selector.MoveToSlot(0);
+            // set highest available current for the Selector
+            ms::selector.SetCurrents(mg::globals.CutIRunCurrent(), config::selector.iHold);
+            // lower move speed
+            savedSelectorFeedRate_mm_s = mg::globals.SelectorFeedrate_mm_s().v;
+            mg::globals.SetSelectorFeedrate_mm_s(config::selectorHomingFeedrate.v);
+            MoveSelector(cutSlot); // let it cut :)
         }
         break;
     case ProgressCode::PerformingCut:
-        if (ms::selector.Slot() == 0) { // this may not be necessary if we want the selector and pulley move at once
+        if (ms::selector.Slot() == cutSlot) { // this may not be necessary if we want the selector and pulley move at once
             state = ProgressCode::ReturningSelector;
-            ms::selector.MoveToSlot(5); // move selector to the other end of its axis to cleanup
+            // revert current to Selector's normal value
+            ms::selector.SetCurrents(config::selector.iRun, config::selector.iHold);
+            // revert move speed
+            mg::globals.SetSelectorFeedrate_mm_s(savedSelectorFeedRate_mm_s);
+            ms::selector.InvalidateHoming();
+            mpu::pulley.PlanMove(-config::cuttingEdgeRetract, config::pulleySlowFeedrate);
         }
         break;
     case ProgressCode::ReturningSelector:
-        if (ms::selector.Slot() == 5) { // selector returned to position, feed the filament back to FINDA
+        if (ms::selector.HomingValid()) { // selector rehomed
             FinishedOK();
             ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::on, ml::off);
-            feed.Reset(true, true);
         }
         break;
     case ProgressCode::OK:
         return true;
+    case ProgressCode::ERRDisengagingIdler:
+        ErrDisengagingIdler();
+        return false;
+    case ProgressCode::ERRWaitingForUser: {
+        // waiting for user buttons and/or a command from the printer
+        mui::Event ev = mui::userInput.ConsumeEvent();
+        switch (ev) {
+        case mui::Event::Middle: // try again the whole sequence
+            InvalidateHoming();
+            Reset(cutSlot);
+            break;
+        default: // no event, continue waiting for user input
+            break;
+        }
+        return false;
+    }
+    case ProgressCode::ERREngagingIdler:
+        if (mi::idler.Engaged()) {
+            state = ProgressCode::ERRHelpingFilament;
+        }
+        return false;
     default: // we got into an unhandled state, better report it
         state = ProgressCode::ERRInternal;
         error = ErrorCode::INTERNAL;

src/logic/cut_filament.h

@@ -4,6 +4,7 @@
 #include "command_base.h"
 #include "unload_filament.h"
 #include "feed_to_finda.h"
+#include "retract_from_finda.h"
 
 namespace logic {
 
@@ -29,9 +30,12 @@ private:
     constexpr static const uint16_t cutStepsPost = 150;
     UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
     FeedToFinda feed;
+    RetractFromFinda retract;
     uint8_t cutSlot;
+    uint16_t savedSelectorFeedRate_mm_s;
 
     void SelectFilamentSlot();
+    void MoveSelector(uint8_t slot);
 };
 
 /// The one and only instance of CutFilament state machine in the FW

src/logic/load_filament.cpp

@@ -80,7 +80,7 @@ bool LoadFilament::StepInner() {
                 // as requested in MMU-116 - stopping an unsuccessful feed should retract as well but not check the filament
                 result = ResultCode::Cancelled;
                 verifyLoadedFilament = 0;
-                // [[fallthrough]]
+                [[fallthrough]];
             case FeedToFinda::OK:
                 GoToRetractingFromFinda();
                 break;

src/modules/globals.cpp

@@ -32,6 +32,8 @@ void Globals::Init() {
     ResetSelectorFeedrate();
 
     ResetIdlerFeedrate();
+
+    ResetCutIRunCurrent();
 }
 
 uint8_t Globals::ActiveSlot() const {

src/modules/globals.h

@@ -95,6 +95,11 @@ public:
     /// Stores the new StallGuard threshold for an axis into EEPROM (does not affect the current state of TMC drivers at all)
     void SetStallGuardThreshold(config::Axis axis, uint8_t sgthrs);
 
+    /// @returns Cut iRun current level (value for TMC2130)
+    uint8_t CutIRunCurrent() const { return cutIRunCurrent; }
+    void ResetCutIRunCurrent() { cutIRunCurrent = config::selectorCutIRun; }
+    void SetCutIRunCurrent(uint8_t v) { cutIRunCurrent = v; }
+
 private:
     /// Sets the active slot, usually after some command/operation.
     /// Also updates the EEPROM records accordingly
@@ -114,6 +119,8 @@ private:
     uint16_t selectorFeedrate_mm_s;
 
     uint16_t idlerFeedrate_deg_s;
+
+    uint8_t cutIRunCurrent;
 };
 
 /// The one and only instance of global state variables

src/modules/idler.cpp

@@ -54,11 +54,9 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() {
 void Idler::FinishMove() {
     currentlyEngaged = plannedMove;
     if (Disengaged()) // reduce power into the Idler motor when disengaged (less force necessary)
-        mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, mm::axisParams[axis].currents);
+        SetCurrents(config::idler.iRun, config::idler.iHold);
     else if (Engaged()) { // maximum motor power when the idler is engaged
-        hal::tmc2130::MotorCurrents tempCurrent = mm::axisParams[axis].currents;
+        SetCurrents(config::idler.iRun, config::idler.iRun);
-        tempCurrent.iHold = tempCurrent.iRun;
-        mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, tempCurrent);
     }
 }
 

src/modules/movable_base.cpp

@@ -23,6 +23,13 @@ void MovableBase::PlanHome() {
     PlanHomingMoveForward();
 }
 
+void MovableBase::SetCurrents(uint8_t iRun, uint8_t iHold) {
+    hal::tmc2130::MotorCurrents tempCurrent = mm::axisParams[axis].currents;
+    tempCurrent.iRun = iRun;
+    tempCurrent.iHold = iHold;
+    mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, tempCurrent);
+}
+
 MovableBase::OperationResult MovableBase::InitMovement() {
     if (motion.InitAxis(axis)) {
         return InitMovementNoReinitAxis();

src/modules/movable_base.h

@@ -68,6 +68,10 @@ public:
 
     inline config::Axis Axis() const { return axis; }
 
+    /// Set TMC2130 iRun current level for this axis
+    /// iRun == 0 means set the default from config
+    void __attribute__((noinline)) SetCurrents(uint8_t iRun, uint8_t iHold);
+
 #ifndef UNITTEST
 protected:
 #endif

src/registers.cpp

@@ -158,6 +158,7 @@
 | 0x1ah 26 | uint16   | Get Pulley position        | 0000h 0      | ffffh 65535 | unit mm                                  | Read only    | M707 A0x1a | N/A
 | 0x1bh 27 | uint16   | Set/Get_Selector_slot      | 0000h 0      | ffffh 65535 | unit slot [0-4/5] 5=park pos             | Read / Write | M707 A0x1b | M708 A0x1b Xn
 | 0x1ch 28 | uint16   | Set/Get_Idler_slot         | 0000h 0      | ffffh 65535 | unit slot [0-4/5] 5=disengaged           | Read / Write | M707 A0x1c | M708 A0x1c Xn
+| 0x1dh 29 | uint8    | Set/Get Selector cut iRun current | 0 to 63 (aka 0-1024mA)| 31 (530mA) |                | Read / Write | M707 A0x1d | M708 A0x1d Xn
 */
 
 struct RegisterFlags {
@@ -368,7 +369,11 @@ static const RegisterRec registers[] /*PROGMEM*/ = {
         []() -> uint16_t { return mi::idler.Slot(); },
         [](uint16_t d) { d >= config::toolCount ? mi::idler.Disengage() : mi::idler.Engage(d); },
         1),
-
+    // 0x1d Set/Get Selector cut iRun current level RW
+    RegisterRec(
+        []() -> uint16_t { return mg::globals.CutIRunCurrent(); },
+        [](uint16_t d) { mg::globals.SetCutIRunCurrent(d); },
+        1),
 };
 
 static constexpr uint8_t registersSize = sizeof(registers) / sizeof(RegisterRec);

tests/unit/logic/cut_filament/test_cut_filament.cpp

@@ -15,6 +15,7 @@
 
 #include "../../modules/stubs/stub_adc.h"
 
+#include "../stubs/homing.h"
 #include "../stubs/main_loop_stub.h"
 #include "../stubs/stub_motion.h"
 
@@ -22,18 +23,18 @@ using Catch::Matchers::Equals;
 
 #include "../helpers/helpers.ipp"
 
-void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
+void CutSlot(logic::CutFilament &cf, uint8_t startSlot, uint8_t cutSlot) {
 
     ForceReinitAllAutomata();
-    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
+    REQUIRE(EnsureActiveSlotIndex(startSlot, mg::FilamentLoadState::AtPulley));
 
-    REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off));
+    REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), startSlot, false, false, ml::off, ml::off));
 
     // restart the automaton
     cf.Reset(cutSlot);
 
     // check initial conditions
-    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::SelectingFilamentSlot));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), startSlot, false, false, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::SelectingFilamentSlot));
 
     // now cycle at most some number of cycles (to be determined yet) and then verify, that the idler and selector reached their target positions
     // Beware - with the real positions of the selector, the number of steps needed to finish some states grows, so the ~40K steps here has a reason
@@ -52,7 +53,7 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
         return cf.TopLevelState() == ProgressCode::FeedingToFinda; }, 5000));
 
     // filament fed to FINDA
-    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::InSelector, cutSlot, cutSlot, true, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToPulley));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::InSelector, cutSlot, cutSlot, true, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
 
     // pull it back to the pulley + simulate FINDA depress
     REQUIRE(WhileCondition(
@@ -61,7 +62,7 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
         if( step == 100 ){ // simulate FINDA trigger - will get depressed in 100 steps
             hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
         }
-        return cf.TopLevelState() == ProgressCode::UnloadingToPulley; }, 5000));
+        return cf.TopLevelState() == ProgressCode::RetractingFromFinda; }, 5000));
 
     REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PreparingBlade));
 
@@ -71,21 +72,28 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
 
     // pushing filament a bit for a cut
     REQUIRE(WhileTopState(cf, ProgressCode::PushingFilament, 5000));
-    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PerformingCut));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler));
+    // disengage idler (to save power for the selector)
+    REQUIRE(WhileTopState(cf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PerformingCut));
 
     // cutting
     REQUIRE(WhileTopState(cf, ProgressCode::PerformingCut, selectorMoveMaxSteps));
-    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, 0, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::ReturningSelector));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), cutSlot, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::ReturningSelector));
+
+    // rehoming selector
+    SimulateSelectorHoming(cf);
 
-    // moving selector to the other end of its axis
     REQUIRE(WhileTopState(cf, ProgressCode::ReturningSelector, selectorMoveMaxSteps));
-    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, ms::Selector::IdleSlotIndex(), false, true, cutSlot, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::OK));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), ms::Selector::IdleSlotIndex(), false, true, cutSlot, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK));
 }
 
 TEST_CASE("cut_filament::cut0", "[cut_filament]") {
+    for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) {
         for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) {
             logic::CutFilament cf;
-        CutSlot(cf, cutSlot);
+            CutSlot(cf, startSlot, cutSlot);
+        }
     }
 }
 
@@ -102,12 +110,16 @@ TEST_CASE("cut_filament::state_machine_reusal", "[cut_filament]") {
         InvalidSlot<logic::CutFilament>(cf, activeSlot, config::toolCount);
     }
 
+    for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) {
         for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) {
-        CutSlot(cf, cutSlot);
+            CutSlot(cf, startSlot, cutSlot);
+        }
     }
 
+    for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) {
         for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) {
-        CutSlot(cf, cutSlot);
+            CutSlot(cf, startSlot, cutSlot);
             InvalidSlot<logic::CutFilament>(cf, cutSlot, config::toolCount);
         }
+    }
 }

tests/unit/logic/helpers/helpers.ipp

@@ -122,7 +122,8 @@ bool VerifyState(SM &uf, mg::FilamentLoadState fls, uint8_t idlerSlotIndex, uint
     CHECKED_ELSE(uf.Error() == err) {
         return false;
     }
-    CHECKED_ELSE(uf.TopLevelState() == topLevelProgress) {
+    auto tls = uf.TopLevelState();
+    CHECKED_ELSE(tls == topLevelProgress) {
     return false;
     }
     return true;