Fix unit tests

- fix homing procedure for Idler and Selector
  (homing now ends with a move to the Parking position)
- fix unit tests' startup conditions with regard to necessary
  homing of Idler and Selector

TODO: still test_cut_filament fails for minor reasons

src/config/config.h

@@ -126,14 +126,14 @@ static constexpr AxisConfig selector = {
 
 /// Selector motion limits
 static constexpr SelectorLimits selectorLimits = {
-    .lenght = 75.0_mm,
+    .lenght = 75.0_mm, // @@TODO how does this relate to SelectorOffsetFromMin?
     .jerk = 1.0_mm_s,
     .accel = 200.0_mm_s2,
 };
 
 static constexpr U_mm SelectorSlotDistance = 14.0_mm; /// Selector distance between two slots
-static constexpr U_mm SelectorOffsetFromMax = 1.0_mm; /// Selector offset from home max to slot 0
-//static constexpr U_mm SelectorOffsetFromMin = 75.5_mm; /// Selector offset from home min to slot 0
+//static constexpr U_mm SelectorOffsetFromMax = 1.0_mm; /// Selector offset from home max to slot 0
+static constexpr U_mm SelectorOffsetFromMin = 75.5_mm; /// Selector offset from home min to slot 0
 
 /// slots 0-4 are the real ones, the 5th is the farthest parking positions
 /// selector.dirOn = true = Home at max: selector hits left side of the MMU2S body
@@ -141,21 +141,20 @@ static constexpr U_mm SelectorOffsetFromMax = 1.0_mm; /// Selector offset from h
 static constexpr U_mm selectorSlotPositions[toolCount + 1] = {
 
     ///selctor max positions
-    SelectorOffsetFromMax + 0 * SelectorSlotDistance, ///1.0_mm + 0 * 14.0_mm = 1.0_mm
-    SelectorOffsetFromMax + 1 * SelectorSlotDistance, ///1.0_mm + 1 * 14.0_mm = 15.0_mm
-    SelectorOffsetFromMax + 2 * SelectorSlotDistance, ///1.0_mm + 2 * 14.0_mm = 29.0_mm
-    SelectorOffsetFromMax + 3 * SelectorSlotDistance, ///1.0_mm + 3 * 14.0_mm = 43.0_mm
-    SelectorOffsetFromMax + 4 * SelectorSlotDistance, ///1.0_mm + 4 * 14.0_mm = 57.0_mm
-    SelectorOffsetFromMax + 5 * SelectorSlotDistance ///1.0_mm + 5 * 14.0_mm = 71.0_mm
-    /*
-///selector min positions
+    //    SelectorOffsetFromMax + 0 * SelectorSlotDistance, ///1.0_mm + 0 * 14.0_mm = 1.0_mm
+    //    SelectorOffsetFromMax + 1 * SelectorSlotDistance, ///1.0_mm + 1 * 14.0_mm = 15.0_mm
+    //    SelectorOffsetFromMax + 2 * SelectorSlotDistance, ///1.0_mm + 2 * 14.0_mm = 29.0_mm
+    //    SelectorOffsetFromMax + 3 * SelectorSlotDistance, ///1.0_mm + 3 * 14.0_mm = 43.0_mm
+    //    SelectorOffsetFromMax + 4 * SelectorSlotDistance, ///1.0_mm + 4 * 14.0_mm = 57.0_mm
+    //    SelectorOffsetFromMax + 5 * SelectorSlotDistance ///1.0_mm + 5 * 14.0_mm = 71.0_mm
+
+    ///selector min positions
     SelectorOffsetFromMin - 1.0_mm - 0 * SelectorSlotDistance, ///75.5_mm - 1.0_mm - 0 * 14.0_mm = 74.5_mm
     SelectorOffsetFromMin - 1.0_mm - 1 * SelectorSlotDistance, ///75.5_mm - 1.0_mm - 1 * 14.0_mm = 60.5_mm
     SelectorOffsetFromMin - 1.0_mm - 2 * SelectorSlotDistance, ///75.5_mm - 1.0_mm - 2 * 14.0_mm = 46.5_mm
     SelectorOffsetFromMin - 1.0_mm - 3 * SelectorSlotDistance, ///75.5_mm - 1.0_mm - 3 * 14.0_mm = 32.5_mm
     SelectorOffsetFromMin - 1.0_mm - 4 * SelectorSlotDistance, ///75.5_mm - 1.0_mm - 4 * 14.0_mm = 18.5_mm
     SelectorOffsetFromMin - 1.0_mm - 5 * SelectorSlotDistance ///75.5_mm - 1.0_mm - 5 * 14.0_mm = 4.5_mm
-*/
 };
 
 static constexpr U_mm_s selectorFeedrate = 30._mm_s;

src/hal/tmc2130.cpp

@@ -2,7 +2,7 @@
 #include "tmc2130.h"
 #include "../config/config.h"
 
-#include "debug.h"
+#include "../debug.h"
 
 namespace hal {
 namespace tmc2130 {

src/main.cpp

@@ -145,8 +145,11 @@ void setup() {
 
     mu::cdc.Init();
 
-    ms::selector.Home();
-    mi::idler.Home();
+    if (mg::globals.FilamentLoaded() < mg::FilamentLoadState::InFSensor) {
+        // home the Selector only in case we don't have filament loaded (or at least we think we don't)
+        ms::selector.Home();
+    }
+    mi::idler.Home(); // home Idler every time
 
     _delay_ms(100);
 

src/modules/idler.cpp

@@ -21,8 +21,17 @@ void Idler::PlanHomingMove() {
     dbg_logic_P(PSTR("Plan Homing Idler"));
 }
 
-void Idler::FinishHoming() {
+void Idler::FinishHomingAndPlanMoveToParkPos() {
     mm::motion.SetPosition(mm::Idler, 0);
+    plannedSlot = IdleSlotIndex();
+    plannedEngage = false;
+    InitMovement(mm::Idler);
+}
+
+void Idler::FinishMove() {
+    currentlyEngaged = plannedEngage;
+    if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary)
+        mm::motion.Disable(mm::Idler);
 }
 
 Idler::OperationResult Idler::Disengage() {
@@ -76,13 +85,6 @@ bool Idler::Step() {
         PerformHome(mm::Idler);
         return false;
     case Ready:
-        // dbg_logic_P(PSTR("Idler Ready"));
-        currentlyEngaged = plannedEngage;
-        currentSlot = plannedSlot;
-
-        if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary)
-            mm::motion.Disable(mm::Idler);
-
         return true;
     case Failed:
         dbg_logic_P(PSTR("Idler Failed"));

src/modules/idler.h

@@ -52,7 +52,8 @@ public:
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
     virtual void PlanHomingMove() override;
-    virtual void FinishHoming() override;
+    virtual void FinishHomingAndPlanMoveToParkPos() override;
+    virtual void FinishMove() override;
 
 private:
     /// direction of travel - engage/disengage

src/modules/motion.h

@@ -176,7 +176,14 @@ public:
     /// Set the position of an axis. Should only be called when the queue is empty.
     /// @param axis axis affected
     /// @param x position to set
-    void SetPosition(Axis axis, pos_t x) { axisData[axis].ctrl.SetPosition(x); }
+    void SetPosition(Axis axis, pos_t x)
+#if !defined(UNITTEST) || defined(UNITTEST_MOTION)
+    {
+        axisData[axis].ctrl.SetPosition(x);
+    }
+#else
+        ;
+#endif
 
     /// Get current acceleration for the selected axis
     /// @param axis axis affected

src/modules/movable_base.cpp

@@ -35,6 +35,8 @@ void MovableBase::PerformMove(config::Axis axis) {
         state = Failed;
     } else if (mm::motion.QueueEmpty(axis)) {
         // move finished
+        currentSlot = plannedSlot;
+        FinishMove();
         state = Ready;
     }
 }
@@ -44,8 +46,8 @@ 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);
-        FinishHoming();
-        state = Ready;
+        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
         mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal);

src/modules/movable_base.h

@@ -60,7 +60,8 @@ protected:
 
     virtual void PrepareMoveToPlannedSlot() = 0;
     virtual void PlanHomingMove() = 0;
-    virtual void FinishHoming() = 0;
+    virtual void FinishHomingAndPlanMoveToParkPos() = 0;
+    virtual void FinishMove() = 0;
 
     OperationResult InitMovement(config::Axis axis);
 

src/modules/selector.cpp

@@ -21,8 +21,14 @@ void Selector::PlanHomingMove() {
     dbg_logic_P(PSTR("Plan Homing Selector"));
 }
 
-void Selector::FinishHoming() {
+void Selector::FinishHomingAndPlanMoveToParkPos() {
     mm::motion.SetPosition(mm::Selector, mm::unitToSteps<mm::S_pos_t>(config::selectorLimits.lenght));
+    plannedSlot = IdleSlotIndex();
+    InitMovement(mm::Selector);
+}
+
+void Selector::FinishMove() {
+    mm::motion.Disable(mm::Selector); // turn off selector motor's power every time
 }
 
 Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
@@ -58,8 +64,6 @@ bool Selector::Step() {
         return false;
     case Ready:
         //dbg_logic_P(PSTR("Selector Ready"));
-        currentSlot = plannedSlot;
-        mm::motion.Disable(mm::Selector); // turn off selector motor's power every time
         return true;
     case Failed:
         dbg_logic_P(PSTR("Selector Failed"));

src/modules/selector.h

@@ -41,7 +41,8 @@ public:
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
     virtual void PlanHomingMove() override;
-    virtual void FinishHoming() override;
+    virtual void FinishHomingAndPlanMoveToParkPos() override;
+    virtual void FinishMove() override;
 
 private:
 };

tests/unit/logic/cut_filament/test_cut_filament.cpp

@@ -24,16 +24,15 @@ using Catch::Matchers::Equals;
 void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
 
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off));
 
-    EnsureActiveSlotIndex(cutSlot);
-
     // restart the automaton
     cf.Reset(cutSlot);
 
     // check initial conditions
-    REQUIRE(VerifyState(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), cutSlot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::SelectingFilamentSlot));
+    REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, 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

tests/unit/logic/eject_filament/test_eject_filament.cpp

@@ -26,6 +26,7 @@ TEST_CASE("eject_filament::eject0", "[eject_filament][.]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     EjectFilament ef;
     // restart the automaton

tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp

@@ -25,6 +25,7 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     FeedToBondtech fb;
     main_loop();

tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp

@@ -25,6 +25,7 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     FeedToFinda ff;
     main_loop();
@@ -92,6 +93,7 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     FeedToFinda ff;
     main_loop();

tests/unit/logic/helpers/helpers.ipp

@@ -69,7 +69,9 @@ bool VerifyState2(SM &uf, mg::FilamentLoadState fls, uint8_t idlerSlotIndex, uin
     CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex).v) {
     return false;
     }
-    CHECKED_ELSE(mi::idler.Engaged() == (idlerSlotIndex < config::toolCount)) { return false; }
+    CHECKED_ELSE(mi::idler.Engaged() == (idlerSlotIndex < config::toolCount)) {
+    return false;
+    }
     CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex).v) {
     return false;
     }
@@ -116,8 +118,9 @@ bool VerifyState2(SM &uf, mg::FilamentLoadState fls, uint8_t idlerSlotIndex, uin
 template<typename SM>
 void InvalidSlot(SM &logicSM,  uint8_t activeSlot, uint8_t invSlot){
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(5);
 
-    REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off));
+    REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), ms::Selector::IdleSlotIndex(), false, false, ml::off, ml::off));
 
     EnsureActiveSlotIndex(activeSlot);
 

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -83,7 +83,7 @@ TEST_CASE("load_filament::regular_load_to_slot_0-4", "[load_filament]") {
 void FailedLoadToFinda(uint8_t slot, logic::LoadFilament &lf) {
     // Stage 2 - feeding to finda
     // we'll assume the finda is defective here and does not trigger
-    REQUIRE(WhileTopState(lf, ProgressCode::FeedingToFinda, 5000));
+    REQUIRE(WhileTopState(lf, ProgressCode::FeedingToFinda, 50000));
     REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRDisengagingIdler));
 
     // Stage 3 - disengaging idler in error mode

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -69,7 +69,26 @@ void ForceReinitAllAutomata() {
     mg::globals.SetActiveSlot(0);
 }
 
+void HomeIdlerAndSelector() {
+    ms::selector.Home();
+    mi::idler.Home();
+
+    // do 5 steps until we trigger the simulated stallguard
+    for (uint8_t i = 0; i < 5; ++i) {
+        main_loop();
+    }
+
+    mm::TriggerStallGuard(mm::Selector);
+    mm::TriggerStallGuard(mm::Idler);
+
+    // 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();
+}
+
 void EnsureActiveSlotIndex(uint8_t slot) {
+    HomeIdlerAndSelector();
+
     // move selector to the right spot
     ms::selector.MoveToSlot(slot);
     while (ms::selector.Slot() != slot)

tests/unit/logic/stubs/stub_motion.cpp

@@ -32,6 +32,10 @@ void Motion::StallGuardReset(Axis axis) {
     axes[axis].stallGuard = false;
 }
 
+void TriggerStallGuard(Axis axis) {
+    axes[axis].stallGuard = true;
+}
+
 void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {
     axes[axis].targetPos = pos;
 }
@@ -40,9 +44,9 @@ pos_t Motion::Position(Axis axis) const {
     return axes[axis].pos;
 }
 
-//void Motion::Home(Axis axis, bool direction) {
-//    axes[Pulley].homed = true;
-//}
+void Motion::SetPosition(Axis axis, pos_t x) {
+    axes[axis].pos = x;
+}
 
 void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) {
 }

tests/unit/logic/stubs/stub_motion.h

@@ -14,8 +14,9 @@ struct AxisSim {
 
 extern AxisSim axes[3];
 
-extern void ReinitMotion();
+void ReinitMotion();
 bool PulleyEnabled();
+void TriggerStallGuard(Axis axis);
 
 } // namespace motion
 } // namespace modules

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -127,7 +127,7 @@ void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) {
     // run the automaton
     // Stage 1 - unloading to FINDA - do NOT let it trigger - keep it pressed, the automaton should finish all moves with the pulley
     // without reaching the FINDA and report an error
-    REQUIRE(WhileTopState(uf, ProgressCode::UnloadingToFinda, 50000));
+    REQUIRE(WhileTopState(uf, ProgressCode::UnloadingToFinda, 500000));
 
     // we still think we have filament loaded at this stage
     // idler should have been activated by the underlying automaton

tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp

@@ -23,6 +23,7 @@ namespace ha = hal::adc;
 
 TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
@@ -62,6 +63,7 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
 
 TEST_CASE("unload_to_finda::no_sense_FINDA_upon_start", "[unload_to_finda]") {
     ForceReinitAllAutomata(); // that implies FINDA OFF which should really not happen for an unload call
+    EnsureActiveSlotIndex(0);
 
     logic::UnloadToFinda ff;
 
@@ -75,6 +77,7 @@ TEST_CASE("unload_to_finda::no_sense_FINDA_upon_start", "[unload_to_finda]") {
 
 TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]") {
     ForceReinitAllAutomata();
+    EnsureActiveSlotIndex(0);
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);

tests/unit/modules/stubs/stub_tmc2130.cpp

@@ -18,9 +18,6 @@ void TMC2130::SetEnabled(const MotorParams &params, bool enabled) {
     this->enabled = enabled;
 }
 
-void TMC2130::ClearStallguard(const MotorParams &params) {
-}
-
 void TMC2130::Isr(const MotorParams &params) {
 }