Cut filament prototype state machine

CMakeLists.txt

@@ -193,11 +193,14 @@ target_sources(
           src/modules/buttons.cpp
           src/modules/debouncer.cpp
           src/modules/finda.cpp
+          src/modules/idler.cpp
           src/modules/leds.cpp
           src/modules/motion.cpp
+          src/modules/selector.cpp
           src/logic/command_base.cpp
           src/logic/cut_filament.cpp
           src/logic/eject_filament.cpp
+          src/logic/feed_to_finda.cpp
           src/logic/load_filament.cpp
           src/logic/no_command.cpp
           src/logic/tool_change.cpp

src/logic/cut_filament.cpp

@@ -1,23 +1,106 @@
 #include "cut_filament.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/idler.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
+#include "../modules/selector.h"
 
 namespace logic {
 
 CutFilament cutFilament;
 
+namespace mm = modules::motion;
+namespace mi = modules::idler;
+namespace ms = modules::selector;
+
 void CutFilament::Reset() {
-    namespace mm = modules::motion;
-    state = ProgressCode::EngagingIdler;
     error = ErrorCode::OK;
+
+    bool isFilamentLoaded = true; //@@TODO
+
+    if (isFilamentLoaded) {
+        state = ProgressCode::CutUnloadingFilament;
+        unl.Reset();
+    } else {
+        SelectFilamentSlot();
+    }
+}
+
+void CutFilament::SelectFilamentSlot() {
+    state = ProgressCode::SelectingFilamentSlot;
+    uint8_t newFilamentSlot = 0; //@@TODO
+    mi::idler.Engage(newFilamentSlot);
+    ms::selector.MoveToSlot(newFilamentSlot);
 }
 
 bool CutFilament::Step() {
-    namespace mm = modules::motion;
+    const int cut_steps_pre = 700;
+    const int cut_steps_post = 150;
+
     switch (state) {
+    case ProgressCode::CutUnloadingFilament:
+        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::SelectingFilamentSlot:
+        if (mm::motion.QueueEmpty()) { // idler and selector finished their moves
+            feed.Reset(true);
+            state = ProgressCode::FeedingToFINDA;
+        }
+        break;
+    case ProgressCode::FeedingToFINDA: // @@TODO this state will be reused for repeated cutting of filament ... probably there will be multiple attempts, not sure
+        if (feed.Step()) {
+            if (feed.State() == FeedToFinda::Failed) {
+                // @@TODO
+            } else {
+                // move selector aside - prepare the blade into active position
+                state = ProgressCode::PreparingBlade;
+                uint8_t newFilamentSlot = 1; //@@TODO
+                ms::selector.MoveToSlot(newFilamentSlot + 1);
+            }
+        }
+        break;
+    case ProgressCode::PreparingBlade:
+        if (mm::motion.QueueEmpty()) {
+            state = ProgressCode::EngagingIdler;
+            uint8_t newFilamentSlot = 0; //@@TODO
+            mi::idler.Engage(newFilamentSlot);
+        }
+        break;
+    case ProgressCode::EngagingIdler:
+        if (mi::idler.Engaged()) {
+            state = ProgressCode::PushingFilament;
+            mm::motion.PlanMove(cut_steps_pre, 0, 0, 1500, 0, 0); //@@TODO
+        }
+        break;
+    case ProgressCode::PushingFilament:
+        if (mm::motion.QueueEmpty()) {
+            state = ProgressCode::PerformingCut;
+            ms::selector.MoveToSlot(0);
+        }
+        break;
+    case ProgressCode::PerformingCut:
+        if (mm::motion.QueueEmpty()) { // this may not be necessary if we want the selector and pulley move at once
+            state = ProgressCode::ReturningSelector;
+            uint8_t newFilamentSlot = 0; //@@TODO
+            ms::selector.MoveToSlot(newFilamentSlot); // return selector back
+        }
+        break;
+    case ProgressCode::ReturningSelector:
+        if (mm::motion.QueueEmpty()) { // selector returned to position, feed the filament back to FINDA
+            state = ProgressCode::FeedingToFINDA;
+            feed.Reset(true);
+        }
+        break;
     }
     return false;
 }

src/logic/cut_filament.h

@@ -1,7 +1,8 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
-#include "unload_to_finda.h"
+#include "unload_filament.h"
+#include "feed_to_finda.h"
 
 namespace logic {
 
@@ -19,6 +20,10 @@ public:
     bool Step() override;
 
 private:
+    UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
+    FeedToFinda feed;
+
+    void SelectFilamentSlot();
 };
 
 extern CutFilament cutFilament;

src/logic/progress_codes.h

@@ -15,5 +15,13 @@ enum class ProgressCode : uint_fast8_t {
     AvoidingGrind,
     FinishingMoves,
     ERR1DisengagingIdler,
-    ERR1WaitingForUser
+    ERR1WaitingForUser,
+
+    CutUnloadingFilament,
+    SelectingFilamentSlot,
+    FeedingToFINDA,
+    PreparingBlade,
+    PushingFilament,
+    PerformingCut,
+    ReturningSelector,
 };

src/logic/unload_filament.cpp

@@ -10,8 +10,10 @@ namespace logic {
 
 UnloadFilament unloadFilament;
 
+namespace mm = modules::motion;
+namespace mi = modules::idler;
+
 void UnloadFilament::Reset() {
-    namespace mm = modules::motion;
     // unloads filament from extruder - filament is above Bondtech gears
     mm::motion.InitAxis(mm::Pulley);
     state = ProgressCode::EngagingIdler;
@@ -20,8 +22,6 @@ void UnloadFilament::Reset() {
 }
 
 bool UnloadFilament::Step() {
-    namespace mm = modules::motion;
-    namespace mi = modules::idler;
     switch (state) {
     case ProgressCode::EngagingIdler: // state 1 engage idler
         if (mi::idler.Engaged()) { // if idler is in parked position un-park it get in contact with filament

src/logic/unload_filament.h

@@ -11,7 +11,7 @@ class UnloadFilament : public CommandBase {
 public:
     inline UnloadFilament()
         : CommandBase()
-        , unl(3) { Reset(); }
+        , unl(3) {}
 
     /// Restart the automaton
     void Reset() override;

src/logic/unload_to_finda.cpp

@@ -1,15 +1,16 @@
 #include "unload_to_finda.h"
-#include "../modules/motion.h"
-#include "../modules/leds.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/leds.h"
+#include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
 
 namespace logic {
 
+namespace mm = modules::motion;
+namespace mf = modules::finda;
+
 void UnloadToFinda::Reset() {
-    namespace mm = modules::motion;
-    namespace mf = modules::finda;
     // check the inital state of FINDA and plan the moves
     if (mf::finda.Pressed()) {
         state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded
@@ -25,8 +26,6 @@ void UnloadToFinda::Reset() {
 }
 
 bool UnloadToFinda::Step() {
-    namespace mm = modules::motion;
-    namespace mf = modules::finda;
     switch (state) {
     case WaitingForFINDA:
         if (modules::finda::finda.Pressed()) {

src/modules/idler.cpp

@@ -7,12 +7,19 @@
 namespace modules {
 namespace idler {
 
+Idler idler;
+
 namespace mm = modules::motion;
 
 bool Idler::Disengage() {
     if (state == Moving)
         return false;
+
+    if (!Engaged())
+        return true;
+
     plannedEngage = false;
+    mm::motion.InitAxis(mm::Idler);
     // plan move to idle position
     // mm::motion.PlanMove(0, idle_position, 0, 1000, 0, 0); // @@TODO
     state = Moving;
@@ -22,8 +29,13 @@ bool Idler::Disengage() {
 bool Idler::Engage(uint8_t slot) {
     if (state == Moving)
         return false;
+
+    if (Engaged())
+        return true;
+
     plannedSlot = slot;
     plannedEngage = true;
+    mm::motion.InitAxis(mm::Idler);
     // mm::motion.PlanMove(0, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
     state = Moving;
     return true;
@@ -33,6 +45,7 @@ bool Idler::Home() {
     if (state == Moving)
         return false;
     plannedEngage = false;
+    mm::motion.InitAxis(mm::Idler);
     mm::motion.Home(mm::Idler, false);
     return true;
 }
@@ -48,6 +61,10 @@ bool Idler::Step() {
     case Ready:
         currentlyEngaged = plannedEngage;
         currentSlot = plannedSlot;
+
+        if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary)
+            mm::motion.DisableAxis(mm::Idler);
+
         return true;
     case Failed:
     default:

src/modules/idler.h

@@ -16,6 +16,13 @@ public:
         Failed
     };
 
+    inline Idler()
+        : plannedEngage(false)
+        , plannedSlot(0)
+        , state(Ready)
+        , currentSlot(0)
+        , currentlyEngaged(false) {}
+
     // public operations on the idler
 
     /// @retuns false in case an operation is already underway
@@ -47,13 +54,6 @@ private:
     /// current state
     uint8_t currentSlot;
     bool currentlyEngaged;
-
-    inline Idler()
-        : plannedEngage(false)
-        , plannedSlot(0)
-        , state(Ready)
-        , currentSlot(0)
-        , currentlyEngaged(false) {}
 };
 
 extern Idler idler;

src/modules/motion.h

@@ -61,7 +61,8 @@ public:
 /// @@TODO this is subject of discussion and change in the future
 class Motion {
 public:
-    /// Init axis driver
+    /// Init axis driver - @@TODO this should be probably hidden somewhere deeper ... something should manage the axes and their state
+    /// especially when the TMC may get randomly reset (deinited)
     void InitAxis(Axis axis) {}
 
     /// Disable axis motor

src/modules/selector.cpp

@@ -7,12 +7,19 @@
 namespace modules {
 namespace selector {
 
+Selector selector;
+
 namespace mm = modules::motion;
 
 bool Selector::MoveToSlot(uint8_t slot) {
     if (state == Moving)
         return false;
+
+    if (currentSlot == slot)
+        return true;
+
     plannedSlot = slot;
+    mm::motion.InitAxis(mm::Selector);
     // mm::motion.PlanMove(1, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
     state = Moving;
     return true;
@@ -21,6 +28,7 @@ bool Selector::MoveToSlot(uint8_t slot) {
 bool Selector::Home() {
     if (state == Moving)
         return false;
+    mm::motion.InitAxis(mm::Selector);
     mm::motion.Home(mm::Selector, false);
     return true;
 }
@@ -35,6 +43,7 @@ bool Selector::Step() {
         return false;
     case Ready:
         currentSlot = plannedSlot;
+        mm::motion.DisableAxis(mm::Selector); // turn off selector motor's power every time
         return true;
     case Failed:
     default:

src/modules/selector.h

@@ -16,6 +16,11 @@ public:
         Failed
     };
 
+    inline Selector()
+        : state(Ready)
+        , currentSlot(0)
+        , plannedSlot(0) {}
+
     // public operations on the selector
 
     /// @retuns false in case an operation is already underway
@@ -40,11 +45,6 @@ private:
 
     /// current state
     uint8_t currentSlot;
-
-    inline Selector()
-        : state(Ready)
-        , currentSlot(0)
-        , plannedSlot(0) {}
 };
 
 extern Selector selector;