Add more checks (esp. positions of idler and selector)

that resulted in finding several weak spots and now Cut and Eject filament
do not pass the test (which is correct, they really have logical issues)

src/logic/command_base.h

@@ -36,9 +36,20 @@ public:
     /// @returns progress of operation - each automaton consists of several internal states
     /// which should be reported to the user via the printer's LCD
     /// E.g. Tool change: first tries to unload filament, then selects another slot and then tries to load filament
+    ///
+    /// Beware - derived automata report detailed states of underlying state machines if any
+    /// E.g. Eject filament first tries to unload filament, which is a standalone automaton.
+    /// Therefore until the unload is finished, this method will report the internal state of Unload filament.
+    /// The reason for this is to be able to report exactly what is happening to the printer, especially loading and unloading sequences (and errors)
     virtual ProgressCode State() const { return state; }
 
-    /// @returns status of the operation - e.g. RUNNING, OK, or an error code if the operation failed
+    /// @returns progress of operation of only this state machine - regardless of any underlying automata (if any)
+    /// Therefore it is not a vitual method.
+    ProgressCode TopLevelState() const { return state; }
+
+    /// @returns status of the operation - e.g. RUNNING, OK, or an error code if the operation failed.
+    ///
+    /// Beware - the same rule about composite operations as with State() applies to Error() as well.
     /// Please see @ErrorCode for more details
     virtual ErrorCode Error() const { return error; }
 

src/logic/cut_filament.cpp

@@ -36,22 +36,13 @@ void CutFilament::SelectFilamentSlot() {
 }
 
 bool CutFilament::Step() {
-    const int cut_steps_pre = 700;
-    const int cut_steps_post = 150;
-
     switch (state) {
     case ProgressCode::UnloadingFilament:
         if (unl.Step()) {
-            // unloading sequence finished
+            // unloading sequence finished - basically, no errors can occurr here
-            switch (unl.Error()) {
+            // as UnloadFilament should handle all the possible error states on its own
-            case ErrorCode::OK: // finished successfully
+            // There is no way the UnloadFilament to finish in an error state
-            case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error?
+            SelectFilamentSlot();
-            case ErrorCode::FINDA_DIDNT_TRIGGER:
-                break;
-            default:
-                state = ProgressCode::ERRInternal;
-                break;
-            }
         }
         break;
     case ProgressCode::SelectingFilamentSlot:
@@ -61,6 +52,8 @@ bool CutFilament::Step() {
         }
         break;
     case ProgressCode::FeedingToFinda: // @@TODO this state will be reused for repeated cutting of filament ... probably there will be multiple attempts, not sure
+        //@@TODO - this is not correct - when the active slot is +1, the FINDA cannot detect the incoming filament - we can only pray that the filament moves
+        //idler should hold slot 0, while the selector is at slot 1
         if (feed.Step()) {
             if (feed.State() == FeedToFinda::Failed) {
                 // @@TODO
@@ -80,7 +73,7 @@ bool CutFilament::Step() {
     case ProgressCode::EngagingIdler:
         if (mi::idler.Engaged()) {
             state = ProgressCode::PushingFilament;
-            mm::motion.PlanMove(cut_steps_pre, 0, 0, 1500, 0, 0); //@@TODO
+            mm::motion.PlanMove(cutStepsPre, 0, 0, 1500, 0, 0); //@@TODO
         }
         break;
     case ProgressCode::PushingFilament:
@@ -109,4 +102,22 @@ bool CutFilament::Step() {
     return false;
 }
 
+ProgressCode CutFilament::State() const {
+    switch (state) {
+    case ProgressCode::UnloadingFilament:
+        return unl.State(); // report sub-automaton states properly
+    default:
+        return state;
+    }
+}
+
+ErrorCode CutFilament::Error() const {
+    switch (state) {
+    case ProgressCode::UnloadingFilament:
+        return unl.Error(); // report sub-automaton errors properly
+    default:
+        return error;
+    }
+}
+
 } // namespace logic

src/logic/cut_filament.h

@@ -19,7 +19,13 @@ public:
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;
 
+    ProgressCode State() const override;
+
+    ErrorCode Error() const override;
+
 private:
+    constexpr static const uint16_t cutStepsPre = 700;
+    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;
 

src/logic/eject_filament.h

@@ -27,9 +27,9 @@ public:
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;
 
-    virtual ProgressCode State() const override;
+    ProgressCode State() const override;
 
-    virtual ErrorCode Error() const override;
+    ErrorCode Error() const override;
 
 private:
     constexpr static const uint16_t ejectSteps = 500; //@@TODO

src/logic/unload_filament.cpp

@@ -11,32 +11,29 @@ namespace logic {
 
 UnloadFilament unloadFilament;
 
+namespace mb = modules::buttons;
 namespace mm = modules::motion;
 namespace mi = modules::idler;
+namespace ml = modules::leds;
 namespace mg = modules::globals;
 
 void UnloadFilament::Reset(uint8_t param) {
     // unloads filament from extruder - filament is above Bondtech gears
     mm::motion.InitAxis(mm::Pulley);
-    state = ProgressCode::EngagingIdler;
+    state = ProgressCode::UnloadingToFinda;
     error = ErrorCode::OK;
-    modules::idler::idler.Engage(mg::globals.ActiveSlot());
+    unl.Reset(maxRetries);
 }
 
 bool UnloadFilament::Step() {
     switch (state) {
-    case ProgressCode::EngagingIdler: // state 1 engage idler
+    // state 1 engage idler - will be done by the Unload to FINDA state machine
-        if (mi::idler.Engaged()) { // if idler is in parked position un-park it get in contact with filament
-            state = ProgressCode::UnloadingToFinda;
-            unl.Reset();
-        }
-        return false;
     case ProgressCode::UnloadingToFinda: // state 2 rotate pulley as long as the FINDA is on
         if (unl.Step()) {
-            if (unl.state == UnloadToFinda::Failed) {
+            if (unl.State() == UnloadToFinda::Failed) {
                 // couldn't unload to FINDA, report error and wait for user to resolve it
                 state = ProgressCode::ERR1DisengagingIdler;
-                modules::leds::leds.SetMode(mg::globals.ActiveSlot(), modules::leds::red, modules::leds::blink0);
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0);
             } else {
                 state = ProgressCode::DisengagingIdler;
             }
@@ -46,7 +43,8 @@ bool UnloadFilament::Step() {
         return false;
     case ProgressCode::DisengagingIdler:
         if (!mi::idler.Engaged()) {
-            state = ProgressCode::AvoidingGrind;
+            state = ProgressCode::AvoidingGrind; // @@TODO what was this originally? Why are we supposed to move the pulley when the idler is not engaged?
+            // may be the pulley was to move along with the idler?
             //                mm::motion.PlanMove(mm::Pulley, -100, 10); // @@TODO constants
         }
         return false;
@@ -71,19 +69,19 @@ bool UnloadFilament::Step() {
         return false;
     case ProgressCode::ERR1WaitingForUser: {
         // waiting for user buttons and/or a command from the printer
-        bool help = modules::buttons::buttons.ButtonPressed(modules::buttons::Left) /*|| command_help()*/;
+        bool help = mb::buttons.ButtonPressed(mb::Left) /*|| command_help()*/;
-        bool tryAgain = modules::buttons::buttons.ButtonPressed(modules::buttons::Middle) /*|| command_tryAgain()*/;
+        bool tryAgain = mb::buttons.ButtonPressed(mb::Middle) /*|| command_tryAgain()*/;
-        bool userResolved = modules::buttons::buttons.ButtonPressed(modules::buttons::Right) /*|| command_userResolved()*/;
+        bool userResolved = mb::buttons.ButtonPressed(mb::Right) /*|| command_userResolved()*/;
         if (help) {
             // try to manually unload just a tiny bit - help the filament with the pulley
             //@@TODO
         } else if (tryAgain) {
             // try again the whole sequence
-            Reset(0); // @@TODO param
+            Reset(0);
         } else if (userResolved) {
             // problem resolved - the user pulled the fillament by hand
-            modules::leds::leds.SetMode(mg::globals.ActiveSlot(), modules::leds::red, modules::leds::off);
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
-            modules::leds::leds.SetMode(mg::globals.ActiveSlot(), modules::leds::green, modules::leds::on);
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
             //                mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants
             state = ProgressCode::AvoidingGrind;
         }
@@ -92,6 +90,10 @@ bool UnloadFilament::Step() {
     case ProgressCode::OK:
         mg::globals.SetFilamentLoaded(false); // filament unloaded
         return true; // successfully finished
+    default: // we got into an unhandled state, better report it
+        state = ProgressCode::ERRInternal;
+        error = ErrorCode::INTERNAL;
+        return true;
     }
     return false;
 }

src/logic/unload_filament.h

@@ -10,8 +10,7 @@ namespace logic {
 class UnloadFilament : public CommandBase {
 public:
     inline UnloadFilament()
-        : CommandBase()
+        : CommandBase() {}
-        , unl(3) {}
 
     /// Restart the automaton
     void Reset(uint8_t param) override;
@@ -20,6 +19,7 @@ public:
     bool Step() override;
 
 private:
+    constexpr static const uint8_t maxRetries = 3;
     UnloadToFinda unl;
 };
 

src/logic/unload_to_finda.cpp

@@ -1,6 +1,8 @@
 #include "unload_to_finda.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/globals.h"
+#include "../modules/idler.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
@@ -8,34 +10,47 @@
 namespace logic {
 
 namespace mm = modules::motion;
+namespace mg = modules::globals;
 namespace mf = modules::finda;
+namespace mi = modules::idler;
+namespace ml = modules::leds;
+namespace mp = modules::permanent_storage;
 
-void UnloadToFinda::Reset() {
+void UnloadToFinda::Reset(uint8_t maxTries) {
+    this->maxTries = maxTries;
     // check the inital state of FINDA and plan the moves
-    if (mf::finda.Pressed()) {
+    if (!mf::finda.Pressed()) {
         state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded
     } else {
         // FINDA is sensing the filament, plan moves to unload it
-        int unloadSteps = /*BowdenLength::get() +*/ 1100; // @@TODO
+        state = EngagingIdler;
-        const int second_point = unloadSteps - 1300;
+        mi::idler.Engage(mg::globals.ActiveSlot());
-        //            mm::motion.PlanMove(mm::Pulley, -1400, 6000); // @@TODO constants
-        //            mm::motion.PlanMove(mm::Pulley, -1800 + 1400, 2500); // @@TODO constants 1800-1400 = 400
-        //            mm::motion.PlanMove(mm::Pulley, -second_point + 1800, 550); // @@TODO constants
-        state = WaitingForFINDA;
     }
 }
 
 bool UnloadToFinda::Step() {
     switch (state) {
+    case EngagingIdler:
+        if (mi::idler.Engaged()) {
+            state = WaitingForFINDA;
+            int unloadSteps = mp::BowdenLength::get() + 1100;
+            const int second_point = unloadSteps - 1300;
+            mm::motion.PlanMove(mm::Pulley, -1400, 6000); // @@TODO constants
+            mm::motion.PlanMove(mm::Pulley, -1800 + 1400, 2500); // @@TODO constants 1800-1400 = 400
+            mm::motion.PlanMove(mm::Pulley, -second_point + 1800, 550); // @@TODO constants
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
+        }
+        return false;
     case WaitingForFINDA:
-        if (modules::finda::finda.Pressed()) {
+        if (!mf::finda.Pressed()) {
             // detected end of filament
             state = OK;
+            mm::motion.AbortPlannedMoves(); // stop rotating the pulley
         } else if (/*tmc2130_read_gstat() &&*/ mm::motion.QueueEmpty()) {
             // we reached the end of move queue, but the FINDA didn't switch off
-            // two possible causes - grinded filament of malfunctioning FINDA
+            // two possible causes - grinded filament or malfunctioning FINDA
             if (--maxTries) {
-                Reset(); // try again
+                Reset(maxTries); // try again
             } else {
                 state = Failed;
             }

src/logic/unload_to_finda.h

@@ -21,20 +21,25 @@ namespace logic {
 ///\enddot
 struct UnloadToFinda {
     enum {
+        EngagingIdler,
         WaitingForFINDA,
         OK,
         Failed
     };
-    uint8_t state;
+    inline UnloadToFinda()
-    uint8_t maxTries;
+        : maxTries(3) {}
-    inline UnloadToFinda(uint8_t maxTries)
-        : maxTries(maxTries) { Reset(); }
 
     /// Restart the automaton
-    void Reset();
+    void Reset(uint8_t maxTries);
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step();
+
+    inline uint8_t State() const { return state; }
+
+private:
+    uint8_t state;
+    uint8_t maxTries;
 };
 
 } // namespace logic

src/modules/idler.cpp

@@ -7,6 +7,9 @@
 namespace modules {
 namespace idler {
 
+// @@TODO PROGMEM
+uint16_t const Idler::slotPositions[6] = { 1, 2, 3, 4, 5, 0 };
+
 Idler idler;
 
 namespace mm = modules::motion;
@@ -22,7 +25,7 @@ bool Idler::Disengage() {
 
     mm::motion.InitAxis(mm::Idler);
     // plan move to idle position
-    // mm::motion.PlanMove(0, idle_position, 0, 1000, 0, 0); // @@TODO
+    mm::motion.PlanMove(mm::Idler, slotPositions[5] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO
     state = Moving;
     return true;
 }
@@ -38,7 +41,7 @@ bool Idler::Engage(uint8_t slot) {
         return true;
 
     mm::motion.InitAxis(mm::Idler);
-    // mm::motion.PlanMove(0, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
+    mm::motion.PlanMove(mm::Idler, slotPositions[slot] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO
     state = Moving;
     return true;
 }
@@ -55,7 +58,8 @@ bool Idler::Home() {
 bool Idler::Step() {
     switch (state) {
     case Moving:
-        if (mm::motion.QueueEmpty()) {
+        if (mm::motion.QueueEmpty()) { //@@TODO this will block until all axes made their movements,
+            // not sure if that is something we want
             // move finished
             state = Ready;
         }

src/modules/idler.h

@@ -39,10 +39,16 @@ public:
     /// 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; }
+
+    /// @returns currently active slot
     inline uint8_t Slot() const { return currentSlot; }
 
+    /// @returns predefined positions of individual slots
+    inline static uint16_t SlotPosition(uint8_t slot) { return slotPositions[slot]; }
+
 private:
-    constexpr static const uint16_t slotPositions[5] = { 1, 2, 3, 4, 5 }; // @@TODO
+    /// slots 0-4 are the real ones, the 5th is the idle position
+    static const uint16_t slotPositions[6];
 
     /// internal state of the automaton
     uint8_t state;

src/modules/motion.cpp

@@ -18,6 +18,8 @@ void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t
 
 void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {}
 
+uint16_t Motion::CurrentPos(Axis axis) const { return 0; }
+
 void Motion::Home(Axis axis, bool direction) {}
 
 void Motion::SetMode(MotorMode mode) {}

src/modules/motion.h

@@ -86,6 +86,10 @@ public:
     /// @param feedrate maximum feedrate/speed after acceleration
     void PlanMove(Axis axis, int16_t delta, uint16_t feedrate);
 
+    /// @returns current position of an axis
+    /// @param axis axis affected
+    uint16_t CurrentPos(Axis axis) const;
+
     /// Enqueue performing of homing of an axis
     /// @@TODO
     void Home(Axis axis, bool direction);

src/modules/selector.cpp

@@ -7,6 +7,9 @@
 namespace modules {
 namespace selector {
 
+// @@TODO PROGMEM
+const uint16_t Selector::slotPositions[6] = { 1, 2, 3, 4, 5, 6 }; // @@TODO
+
 Selector selector;
 
 namespace mm = modules::motion;
@@ -21,7 +24,7 @@ bool Selector::MoveToSlot(uint8_t slot) {
         return true;
 
     mm::motion.InitAxis(mm::Selector);
-    // mm::motion.PlanMove(1, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
+    mm::motion.PlanMove(mm::Selector, slotPositions[slot] - mm::motion.CurrentPos(mm::Selector), 1000); // @@TODO
     state = Moving;
     return true;
 }

src/modules/selector.h

@@ -36,8 +36,12 @@ public:
     /// state machines to use this call as a waiting condition for the desired state of the selector
     inline uint8_t Slot() const { return currentSlot; }
 
+    /// @returns predefined positions of individual slots
+    inline static uint16_t SlotPosition(uint8_t slot) { return slotPositions[slot]; }
+
 private:
-    constexpr static const uint16_t slotPositions[5] = { 1, 2, 3, 4, 5 }; // @@TODO
+    /// slots 0-4 are the real ones, the 5th is the farthest parking positions
+    static const uint16_t slotPositions[6];
 
     /// internal state of the automaton
     uint8_t state;

tests/unit/logic/CMakeLists.txt

@@ -4,7 +4,7 @@ add_subdirectory(feed_to_finda)
 
 # add_subdirectory(feed_to_bondtech)
 
-# add_subdirectory(unload_to_finda)
+add_subdirectory(unload_to_finda)
 
 add_subdirectory(eject_filament)
 
@@ -14,4 +14,4 @@ add_subdirectory(eject_filament)
 
 # add_subdirectory(cut_filament)
 
-# add_subdirectory(unload_filament)
+add_subdirectory(unload_filament)

tests/unit/logic/cut_filament/test_cut_filament.cpp

@@ -19,13 +19,13 @@
 
 using Catch::Matchers::Equals;
 
-template <typename COND>
+namespace mm = modules::motion;
-bool WhileCondition(COND cond, uint32_t maxLoops = 5000) {
+namespace mf = modules::finda;
-    while (cond() && --maxLoops) {
+namespace mi = modules::idler;
-        main_loop();
+namespace ml = modules::leds;
-    }
+namespace mb = modules::buttons;
-    return maxLoops > 0;
+namespace mg = modules::globals;
-}
+namespace ms = modules::selector;
 
 TEST_CASE("cut_filament::cut0", "[cut_filament]") {
     using namespace logic;
@@ -41,36 +41,39 @@ TEST_CASE("cut_filament::cut0", "[cut_filament]") {
 
     // it should have instructed the selector and idler to move to slot 1
     // check if the idler and selector have the right command
-    CHECK(modules::motion::axes[modules::motion::Idler].targetPos == 0); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Idler].targetPos == mi::Idler::SlotPosition(0));
-    CHECK(modules::motion::axes[modules::motion::Selector].targetPos == 0); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Selector].targetPos == ms::Selector::SlotPosition(0));
 
     // now cycle at most some number of cycles (to be determined yet) and then verify, that the idler and selector reached their target positions
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::SelectingFilamentSlot; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::SelectingFilamentSlot; }, 5000));
+
+    CHECK(modules::motion::axes[modules::motion::Idler].pos == mi::Idler::SlotPosition(0));
+    CHECK(modules::motion::axes[modules::motion::Selector].pos == ms::Selector::SlotPosition(0));
 
     // idler and selector reached their target positions and the CF automaton will start feeding to FINDA as the next step
-    REQUIRE(cf.State() == ProgressCode::FeedingToFinda);
+    REQUIRE(cf.TopLevelState() == ProgressCode::FeedingToFinda);
     // prepare for simulated finda trigger
     hal::adc::ReinitADC(1, hal::adc::TADCData({ 0, 0, 0, 0, 600, 700, 800, 900 }), 10);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::FeedingToFinda; }, 50000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::FeedingToFinda; }, 5000));
 
     // filament fed into FINDA, cutting...
-    REQUIRE(cf.State() == ProgressCode::PreparingBlade);
+    REQUIRE(cf.TopLevelState() == ProgressCode::PreparingBlade);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::PreparingBlade; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::PreparingBlade; }, 5000));
 
-    REQUIRE(cf.State() == ProgressCode::EngagingIdler);
+    REQUIRE(cf.TopLevelState() == ProgressCode::EngagingIdler);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::EngagingIdler; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::EngagingIdler; }, 5000));
 
     // the idler should be at the active slot @@TODO
-    REQUIRE(cf.State() == ProgressCode::PushingFilament);
+    REQUIRE(cf.TopLevelState() == ProgressCode::PushingFilament);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::PushingFilament; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::PushingFilament; }, 5000));
 
     // filament pushed - performing cut
-    REQUIRE(cf.State() == ProgressCode::PerformingCut);
+    REQUIRE(cf.TopLevelState() == ProgressCode::PerformingCut);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::PerformingCut; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::PerformingCut; }, 5000));
 
     // returning selector
-    REQUIRE(cf.State() == ProgressCode::ReturningSelector);
+    REQUIRE(cf.TopLevelState() == ProgressCode::ReturningSelector);
-    REQUIRE(WhileCondition([&]() { return cf.State() == ProgressCode::ReturningSelector; }, 5000));
+    REQUIRE(WhileCondition([&]() { return cf.TopLevelState() == ProgressCode::ReturningSelector; }, 5000));
 
     // the next states are still @@TODO
 }

tests/unit/logic/eject_filament/test_eject_filament.cpp

@@ -19,13 +19,13 @@
 
 using Catch::Matchers::Equals;
 
-template <typename COND>
+namespace mm = modules::motion;
-bool WhileCondition(COND cond, uint32_t maxLoops = 5000) {
+namespace mf = modules::finda;
-    while (cond() && --maxLoops) {
+namespace mi = modules::idler;
-        main_loop();
+namespace ml = modules::leds;
-    }
+namespace mb = modules::buttons;
-    return maxLoops > 0;
+namespace mg = modules::globals;
-}
+namespace ms = modules::selector;
 
 TEST_CASE("eject_filament::eject0", "[eject_filament]") {
     using namespace logic;
@@ -41,36 +41,36 @@ TEST_CASE("eject_filament::eject0", "[eject_filament]") {
 
     // it should have instructed the selector and idler to move to slot 1
     // check if the idler and selector have the right command
-    CHECK(modules::motion::axes[modules::motion::Idler].targetPos == 0); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Idler].targetPos == mi::Idler::SlotPosition(0));
-    CHECK(modules::motion::axes[modules::motion::Selector].targetPos == 0); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Selector].targetPos == ms::Selector::SlotPosition(4));
 
     // now cycle at most some number of cycles (to be determined yet) and then verify, that the idler and selector reached their target positions
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::SelectingFilamentSlot; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::SelectingFilamentSlot; }, 5000));
 
     // idler and selector reached their target positions and the CF automaton will start feeding to FINDA as the next step
-    REQUIRE(ef.State() == ProgressCode::FeedingToFinda);
+    REQUIRE(ef.TopLevelState() == ProgressCode::FeedingToFinda);
     // prepare for simulated finda trigger
     hal::adc::ReinitADC(1, hal::adc::TADCData({ 0, 0, 0, 0, 600, 700, 800, 900 }), 10);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::FeedingToFinda; }, 50000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::FeedingToFinda; }, 50000));
 
     // filament fed into FINDA, cutting...
-    REQUIRE(ef.State() == ProgressCode::PreparingBlade);
+    REQUIRE(ef.TopLevelState() == ProgressCode::PreparingBlade);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::PreparingBlade; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::PreparingBlade; }, 5000));
 
-    REQUIRE(ef.State() == ProgressCode::EngagingIdler);
+    REQUIRE(ef.TopLevelState() == ProgressCode::EngagingIdler);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::EngagingIdler; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::EngagingIdler; }, 5000));
 
     // the idler should be at the active slot @@TODO
-    REQUIRE(ef.State() == ProgressCode::PushingFilament);
+    REQUIRE(ef.TopLevelState() == ProgressCode::PushingFilament);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::PushingFilament; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::PushingFilament; }, 5000));
 
     // filament pushed - performing cut
-    REQUIRE(ef.State() == ProgressCode::PerformingCut);
+    REQUIRE(ef.TopLevelState() == ProgressCode::PerformingCut);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::PerformingCut; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::PerformingCut; }, 5000));
 
     // returning selector
-    REQUIRE(ef.State() == ProgressCode::ReturningSelector);
+    REQUIRE(ef.TopLevelState() == ProgressCode::ReturningSelector);
-    REQUIRE(WhileCondition([&]() { return ef.State() == ProgressCode::ReturningSelector; }, 5000));
+    REQUIRE(WhileCondition([&]() { return ef.TopLevelState() == ProgressCode::ReturningSelector; }, 5000));
 
     // the next states are still @@TODO
 }

tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp

@@ -53,9 +53,9 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
 
     // it should have instructed the selector and idler to move to slot 1
     // check if the idler and selector have the right command
-    CHECK(mm::axes[mm::Idler].targetPos == 0); // @@TODO constants
+    CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
-    CHECK(mm::axes[mm::Selector].targetPos == 0); // @@TODO constants
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
-    CHECK(mm::axes[mm::Idler].enabled == true); // @@TODO constants
+    CHECK(mm::axes[mm::Idler].enabled == true);
 
     // engaging idler
     REQUIRE(WhileCondition(
@@ -63,6 +63,9 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
         [&]() { return !mi::idler.Engaged(); },
         5000));
 
+    CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+
     // idler engaged, selector in position, we'll start pushing filament
     REQUIRE(ff.State() == FeedToFinda::PushingFilament);
     // at least at the beginning the LED should shine green (it should be blinking, but this mode has been already verified in the LED's unit test)
@@ -92,6 +95,9 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
         [&]() { return mi::idler.Engaged(); },
         5000));
 
+    CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // @@TODO constants
+    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+
     // state machine finished ok, the green LED should be on
     REQUIRE(ff.State() == FeedToFinda::OK);
     REQUIRE(ml::leds.LedOn(mg::globals.ActiveSlot(), ml::Color::green));
@@ -114,8 +120,8 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
 
     // it should have instructed the selector and idler to move to slot 1
     // check if the idler and selector have the right command
-    CHECK(mm::axes[mm::Idler].targetPos == 0); // @@TODO constants
+    CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
-    CHECK(mm::axes[mm::Selector].targetPos == 0); // @@TODO constants
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
 
     // engaging idler
     REQUIRE(WhileCondition(
@@ -123,6 +129,9 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
         [&]() { return !mi::idler.Engaged(); },
         5000));
 
+    CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+
     // idler engaged, we'll start pushing filament
     REQUIRE(ff.State() == FeedToFinda::PushingFilament);
     // at least at the beginning the LED should shine green (it should be blinking, but this mode has been already verified in the LED's unit test)

tests/unit/logic/stubs/main_loop_stub.h

@@ -5,3 +5,11 @@ extern void main_loop();
 extern void ForceReinitAllAutomata();
 
 extern logic::CommandBase *currentCommand;
+
+template <typename COND>
+bool WhileCondition(COND cond, uint32_t maxLoops = 5000) {
+    while (cond() && --maxLoops) {
+        main_loop();
+    }
+    return maxLoops > 0;
+}

tests/unit/logic/stubs/stub_motion.cpp

@@ -6,9 +6,9 @@ namespace motion {
 
 Motion motion;
 AxisSim axes[3] = {
-    { 0, 0, false, false, false },
+    { 0, 0, false, false, false }, // pulley
-    { 0, 0, false, false, false },
+    { 1, 1, false, false, false }, // selector //@@TODO proper selector positions once defined
-    { 0, 0, false, false, false },
+    { 0, 0, false, false, false }, // idler
 };
 
 void Motion::InitAxis(Axis axis) {
@@ -38,6 +38,10 @@ void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {
     axes[axis].targetPos = axes[axis].pos + delta;
 }
 
+uint16_t Motion::CurrentPos(Axis axis) const {
+    return axes[axis].pos;
+}
+
 void Motion::Home(Axis axis, bool direction) {
     axes[Pulley].homed = true;
 }

tests/unit/logic/unload_filament/CMakeLists.txt

@@ -0,0 +1,32 @@
+# define the test executable
+add_executable(
+  unload_filament_tests
+  ../../../../src/logic/feed_to_finda.cpp
+  ../../../../src/logic/unload_filament.cpp
+  ../../../../src/logic/unload_to_finda.cpp
+  ../../../../src/modules/buttons.cpp
+  ../../../../src/modules/debouncer.cpp
+  ../../../../src/modules/finda.cpp
+  ../../../../src/modules/fsensor.cpp
+  ../../../../src/modules/globals.cpp
+  ../../../../src/modules/idler.cpp
+  ../../../../src/modules/leds.cpp
+  ../../../../src/modules/permanent_storage.cpp
+  ../../../../src/modules/selector.cpp
+  ../../modules/stubs/stub_adc.cpp
+  ../../modules/stubs/stub_eeprom.cpp
+  ../../modules/stubs/stub_shr16.cpp
+  ../../modules/stubs/stub_timebase.cpp
+  ../stubs/main_loop_stub.cpp
+  ../stubs/stub_motion.cpp
+  test_unload_filament.cpp
+  )
+
+# define required search paths
+target_include_directories(
+  unload_filament_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal
+                               ${CMAKE_SOURCE_DIR}/src/logic
+  )
+
+# tell build system about the test case
+add_catch_test(unload_filament_tests)

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -0,0 +1,56 @@
+#include "catch2/catch.hpp"
+
+#include "../../../../src/modules/buttons.h"
+#include "../../../../src/modules/finda.h"
+#include "../../../../src/modules/fsensor.h"
+#include "../../../../src/modules/globals.h"
+#include "../../../../src/modules/idler.h"
+#include "../../../../src/modules/leds.h"
+#include "../../../../src/modules/motion.h"
+#include "../../../../src/modules/permanent_storage.h"
+#include "../../../../src/modules/selector.h"
+
+#include "../../../../src/logic/unload_filament.h"
+
+#include "../../modules/stubs/stub_adc.h"
+
+#include "../stubs/main_loop_stub.h"
+#include "../stubs/stub_motion.h"
+
+using Catch::Matchers::Equals;
+
+namespace mm = modules::motion;
+namespace mf = modules::finda;
+namespace mi = modules::idler;
+namespace ml = modules::leds;
+namespace mb = modules::buttons;
+namespace mg = modules::globals;
+namespace ms = modules::selector;
+
+TEST_CASE("unload_filament::unload0", "[unload_filament]") {
+    using namespace logic;
+
+    ForceReinitAllAutomata();
+
+    UnloadFilament uf;
+    // restart the automaton
+    currentCommand = &uf;
+    uf.Reset(0);
+
+    main_loop();
+
+    REQUIRE(WhileCondition([&]() { return uf.TopLevelState() == ProgressCode::UnloadingToFinda; }, 5000));
+
+    REQUIRE(uf.TopLevelState() == ProgressCode::DisengagingIdler);
+    REQUIRE(WhileCondition([&]() { return uf.TopLevelState() == ProgressCode::DisengagingIdler; }, 5000));
+
+    CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5));
+
+    REQUIRE(uf.TopLevelState() == ProgressCode::AvoidingGrind);
+    REQUIRE(WhileCondition([&]() { return uf.TopLevelState() == ProgressCode::AvoidingGrind; }, 5000));
+
+    REQUIRE(uf.TopLevelState() == ProgressCode::FinishingMoves);
+    REQUIRE(WhileCondition([&]() { return uf.TopLevelState() == ProgressCode::FinishingMoves; }, 5000));
+
+    REQUIRE(uf.TopLevelState() == ProgressCode::OK);
+}

tests/unit/logic/unload_to_finda/CMakeLists.txt

@@ -0,0 +1,30 @@
+# define the test executable
+add_executable(
+  unload_to_finda_tests
+  ../../../../src/logic/unload_to_finda.cpp
+  ../../../../src/modules/buttons.cpp
+  ../../../../src/modules/debouncer.cpp
+  ../../../../src/modules/finda.cpp
+  ../../../../src/modules/fsensor.cpp
+  ../../../../src/modules/globals.cpp
+  ../../../../src/modules/idler.cpp
+  ../../../../src/modules/leds.cpp
+  ../../../../src/modules/permanent_storage.cpp
+  ../../../../src/modules/selector.cpp
+  ../../modules/stubs/stub_adc.cpp
+  ../../modules/stubs/stub_eeprom.cpp
+  ../../modules/stubs/stub_shr16.cpp
+  ../../modules/stubs/stub_timebase.cpp
+  ../stubs/main_loop_stub.cpp
+  ../stubs/stub_motion.cpp
+  test_unload_to_finda.cpp
+  )
+
+# define required search paths
+target_include_directories(
+  unload_to_finda_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal
+                               ${CMAKE_SOURCE_DIR}/src/logic
+  )
+
+# tell build system about the test case
+add_catch_test(unload_to_finda_tests)

tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp

@@ -0,0 +1,141 @@
+#include "catch2/catch.hpp"
+
+#include "../../../../src/modules/buttons.h"
+#include "../../../../src/modules/finda.h"
+#include "../../../../src/modules/fsensor.h"
+#include "../../../../src/modules/globals.h"
+#include "../../../../src/modules/idler.h"
+#include "../../../../src/modules/leds.h"
+#include "../../../../src/modules/motion.h"
+#include "../../../../src/modules/permanent_storage.h"
+#include "../../../../src/modules/selector.h"
+
+#include "../../../../src/logic/unload_to_finda.h"
+
+#include "../../modules/stubs/stub_adc.h"
+
+#include "../stubs/main_loop_stub.h"
+#include "../stubs/stub_motion.h"
+
+using Catch::Matchers::Equals;
+
+namespace mm = modules::motion;
+namespace mf = modules::finda;
+namespace mi = modules::idler;
+namespace ml = modules::leds;
+namespace mb = modules::buttons;
+namespace mg = modules::globals;
+namespace ms = modules::selector;
+
+namespace ha = hal::adc;
+
+template <typename COND>
+bool WhileConditionFF(logic::UnloadToFinda &ff, COND cond, uint32_t maxLoops = 5000) {
+    while (cond() && --maxLoops) {
+        main_loop();
+        ff.Step();
+    }
+    return maxLoops > 0;
+}
+
+TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
+    using namespace logic;
+
+    ForceReinitAllAutomata();
+
+    // we need finda ON
+    hal::adc::ReinitADC(1, hal::adc::TADCData({ 1023 }), 1);
+
+    UnloadToFinda ff;
+
+    // wait for FINDA to debounce
+    REQUIRE(WhileCondition(
+        [&]() { return !mf::finda.Pressed(); },
+        5000));
+
+    // restart the automaton - just 1 attempt
+    ff.Reset(1);
+
+    REQUIRE(ff.State() == UnloadToFinda::EngagingIdler);
+
+    // it should have instructed the selector and idler to move to slot 1
+    // check if the idler and selector have the right command
+    CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    CHECK(mm::axes[mm::Idler].enabled == true);
+
+    // engaging idler
+    REQUIRE(WhileConditionFF(
+        ff,
+        [&]() { return !mi::idler.Engaged(); },
+        5000));
+
+    // now pulling the filament until finda triggers
+    REQUIRE(ff.State() == UnloadToFinda::WaitingForFINDA);
+    hal::adc::ReinitADC(1, hal::adc::TADCData({ 1023, 900, 800, 500, 0 }), 10);
+    REQUIRE(WhileConditionFF(
+        ff,
+        [&]() { return mf::finda.Pressed(); },
+        50000));
+
+    REQUIRE(ff.State() == UnloadToFinda::OK);
+}
+
+TEST_CASE("unload_to_finda::no_sense_FINDA_upon_start", "[unload_to_finda]") {
+    using namespace logic;
+
+    ForceReinitAllAutomata(); // that implies FINDA OFF which should really not happen for an unload call
+
+    UnloadToFinda ff;
+
+    // restart the automaton - just 1 attempt
+    ff.Reset(1);
+
+    // the state machine should accept the unpressed FINDA as no-fillament-loaded
+    // thus should immediately end in the OK state
+    REQUIRE(ff.State() == UnloadToFinda::OK);
+}
+
+TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]") {
+    using namespace logic;
+
+    ForceReinitAllAutomata();
+
+    // we need finda ON
+    hal::adc::ReinitADC(1, hal::adc::TADCData({ 1023 }), 1);
+
+    UnloadToFinda ff;
+
+    // wait for FINDA to debounce
+    REQUIRE(WhileCondition(
+        [&]() { return !mf::finda.Pressed(); },
+        5000));
+
+    // restart the automaton - just 1 attempt
+    ff.Reset(1);
+
+    REQUIRE(ff.State() == UnloadToFinda::EngagingIdler);
+
+    // it should have instructed the selector and idler to move to slot 1
+    // check if the idler and selector have the right command
+    CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    CHECK(mm::axes[mm::Idler].enabled == true);
+
+    // engaging idler
+    REQUIRE(WhileConditionFF(
+        ff,
+        [&]() { return !mi::idler.Engaged(); },
+        5000));
+
+    // now pulling the filament until finda triggers
+    REQUIRE(ff.State() == UnloadToFinda::WaitingForFINDA);
+
+    // no changes to FINDA during unload - we'll pretend it never triggers
+    REQUIRE(!WhileConditionFF(
+        ff,
+        [&]() { return mf::finda.Pressed(); },
+        50000));
+
+    REQUIRE(ff.State() == UnloadToFinda::Failed);
+}