Add Eject filament operation

+ refactor other state machines a bit - Reset() now has the parameter which comes in the RequestMsg

src/logic/command_base.h

@@ -26,7 +26,8 @@ public:
     // virtual ~CommandBase() = default;
 
     /// resets the automaton
-    virtual void Reset() = 0;
+    /// @param param numerical parameter that comes with some commands (e.g. T1 for tool change 1)
+    virtual void Reset(uint8_t param) = 0;
 
     /// steps the state machine
     /// @returns true if the automaton finished its work

src/logic/cut_filament.cpp

@@ -15,14 +15,14 @@ namespace mm = modules::motion;
 namespace mi = modules::idler;
 namespace ms = modules::selector;
 
-void CutFilament::Reset() {
+void CutFilament::Reset(uint8_t param) {
     error = ErrorCode::OK;
 
     bool isFilamentLoaded = true; //@@TODO
 
     if (isFilamentLoaded) {
-        state = ProgressCode::CutUnloadingFilament;
-        unl.Reset();
+        state = ProgressCode::UnloadingFilament;
+        unl.Reset(param); //@@TODO probably only act on active_extruder
     } else {
         SelectFilamentSlot();
     }
@@ -40,7 +40,7 @@ bool CutFilament::Step() {
     const int cut_steps_post = 150;
 
     switch (state) {
-    case ProgressCode::CutUnloadingFilament:
+    case ProgressCode::UnloadingFilament:
         if (unl.Step()) {
             // unloading sequence finished
             switch (unl.Error()) {

src/logic/cut_filament.h

@@ -11,10 +11,10 @@ namespace logic {
 class CutFilament : public CommandBase {
 public:
     inline CutFilament()
-        : CommandBase() { Reset(); }
+        : CommandBase() {}
 
     /// Restart the automaton
-    void Reset() override;
+    void Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;

src/logic/eject_filament.cpp

@@ -3,21 +3,74 @@
 #include "../modules/finda.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
+#include "../modules/selector.h"
+#include "../modules/idler.h"
 #include "../modules/permanent_storage.h"
 
 namespace logic {
 
 EjectFilament ejectFilament;
 
-void EjectFilament::Reset() {
-    namespace mm = modules::motion;
-    state = ProgressCode::EngagingIdler;
+namespace mm = modules::motion;
+namespace mi = modules::idler;
+namespace ms = modules::selector;
+
+void EjectFilament::Reset(uint8_t param) {
     error = ErrorCode::OK;
+    slot = param;
+
+    bool isFilamentLoaded = true; //@@TODO
+
+    if (isFilamentLoaded) {
+        state = ProgressCode::UnloadingFilament;
+        unl.Reset(param); //@@TODO probably act on active extruder only
+    } else {
+        MoveSelectorAside();
+    }
+}
+
+void EjectFilament::MoveSelectorAside() {
+    state = ProgressCode::ParkingSelector;
+    const uint8_t selectorParkedPos = (slot <= 2) ? 4 : 0;
+    mi::idler.Engage(slot);
+    ms::selector.MoveToSlot(selectorParkedPos);
 }
 
 bool EjectFilament::Step() {
-    namespace mm = modules::motion;
+    constexpr const uint16_t eject_steps = 500; //@@TODO
     switch (state) {
+    case ProgressCode::UnloadingFilament:
+        if (unl.Step()) {
+            // unloading sequence finished
+            switch (unl.Error()) {
+            case ErrorCode::OK: // finished successfully
+            case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error?
+            case ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER:
+                break;
+            }
+        }
+        break;
+    case ProgressCode::ParkingSelector:
+        if (mm::motion.QueueEmpty()) { // selector parked aside
+            state = ProgressCode::EjectingFilament;
+            mm::motion.InitAxis(mm::Pulley);
+            mm::motion.PlanMove(eject_steps, 0, 0, 1500, 0, 0);
+        }
+        break;
+    case ProgressCode::EjectingFilament:
+        if (mm::motion.QueueEmpty()) { // filament ejected
+            state = ProgressCode::DisengagingIdler;
+            mi::idler.Disengage();
+        }
+        break;
+    case ProgressCode::DisengagingIdler:
+        if (mm::motion.QueueEmpty()) { // idler disengaged
+            mm::motion.DisableAxis(mm::Pulley);
+            state = ProgressCode::OK;
+        }
+        break;
+    case ProgressCode::OK:
+        return true;
     }
     return false;
 }

src/logic/eject_filament.h

@@ -1,24 +1,36 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
-#include "unload_to_finda.h"
+#include "unload_filament.h"
 
 namespace logic {
 
 /// A high-level command state machine
-/// Handles the complex logic of ejecting filament
+/// Handles the complex logic of ejecting filament:
+///
+/// - Move selector sideways and push filament forward a little bit, so that the user can catch it
+/// - Unpark idler at the end so that the user can pull filament out.
+/// - If there is still some filament detected by PINDA unload it first.
+/// - If we want to eject fil 0-2, move selector to position 4 (right).
+/// - If we want to eject fil 3-4, move selector to position 0 (left)
+/// Optionally, we can also move the selector to its service position in the future.
+/// @param filament filament 0 to 4
+
 class EjectFilament : public CommandBase {
 public:
     inline EjectFilament()
-        : CommandBase() { Reset(); }
+        : CommandBase() {}
 
     /// Restart the automaton
-    void Reset() override;
+    void Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;
 
 private:
+    UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
+    uint8_t slot;
+    void MoveSelectorAside();
 };
 
 extern EjectFilament ejectFilament;

src/logic/load_filament.cpp

@@ -9,14 +9,14 @@ namespace logic {
 
 LoadFilament loadFilament;
 
-void LoadFilament::Reset() {
-    namespace mm = modules::motion;
+namespace mm = modules::motion;
+
+void LoadFilament::Reset(uint8_t param) {
     state = ProgressCode::EngagingIdler;
     error = ErrorCode::OK;
 }
 
 bool LoadFilament::Step() {
-    namespace mm = modules::motion;
     switch (state) {
     }
     return false;

src/logic/load_filament.h

@@ -10,10 +10,10 @@ namespace logic {
 class LoadFilament : public CommandBase {
 public:
     inline LoadFilament()
-        : CommandBase() { Reset(); }
+        : CommandBase() {}
 
     /// Restart the automaton
-    void Reset() override;
+    void Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;

src/logic/no_command.h

@@ -12,7 +12,7 @@ public:
         : CommandBase() {}
 
     /// Restart the automaton
-    void Reset() override {}
+    void Reset(uint8_t /*param*/) override {}
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override { return true; }

src/logic/progress_codes.h

@@ -17,11 +17,13 @@ enum class ProgressCode : uint_fast8_t {
     ERR1DisengagingIdler,
     ERR1WaitingForUser,
 
-    CutUnloadingFilament,
+    UnloadingFilament,
     SelectingFilamentSlot,
     FeedingToFINDA,
     PreparingBlade,
     PushingFilament,
     PerformingCut,
     ReturningSelector,
+    ParkingSelector,
+    EjectingFilament,
 };

src/logic/tool_change.cpp

@@ -9,7 +9,7 @@ namespace logic {
 
 ToolChange toolChange;
 
-void ToolChange::Reset() {
+void ToolChange::Reset(uint8_t param) {
     namespace mm = modules::motion;
     state = ProgressCode::EngagingIdler;
     error = ErrorCode::OK;

src/logic/tool_change.h

@@ -10,10 +10,10 @@ namespace logic {
 class ToolChange : public CommandBase {
 public:
     inline ToolChange()
-        : CommandBase() { Reset(); }
+        : CommandBase() {}
 
     /// Restart the automaton
-    void Reset() override;
+    void Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;

src/logic/unload_filament.cpp

@@ -13,7 +13,7 @@ UnloadFilament unloadFilament;
 namespace mm = modules::motion;
 namespace mi = modules::idler;
 
-void UnloadFilament::Reset() {
+void UnloadFilament::Reset(uint8_t param) {
     // unloads filament from extruder - filament is above Bondtech gears
     mm::motion.InitAxis(mm::Pulley);
     state = ProgressCode::EngagingIdler;
@@ -77,7 +77,7 @@ bool UnloadFilament::Step() {
             //@@TODO
         } else if (tryAgain) {
             // try again the whole sequence
-            Reset();
+            Reset(0); // @@TODO param
         } else if (userResolved) {
             // problem resolved - the user pulled the fillament by hand
             //                modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::off);

src/logic/unload_filament.h

@@ -14,7 +14,7 @@ public:
         , unl(3) {}
 
     /// Restart the automaton
-    void Reset() override;
+    void Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool Step() override;

src/main.cpp

@@ -149,7 +149,7 @@ void PlanCommand(const modules::protocol::RequestMsg &rq) {
             currentCommand = &logic::noCommand;
             break;
         }
-        currentCommand->Reset();
+        currentCommand->Reset(rq.value);
     }
 }