Add Load filament state machine

+ printer's fsensor (external) module
+ state machine for loading to bondtech

CMakeLists.txt

@@ -193,6 +193,7 @@ target_sources(
           src/modules/buttons.cpp
           src/modules/debouncer.cpp
           src/modules/finda.cpp
+          src/modules/fsensor.cpp
           src/modules/idler.cpp
           src/modules/leds.cpp
           src/modules/motion.cpp
@@ -200,6 +201,7 @@ target_sources(
           src/logic/command_base.cpp
           src/logic/cut_filament.cpp
           src/logic/eject_filament.cpp
+          src/logic/feed_to_bondtech.cpp
           src/logic/feed_to_finda.cpp
           src/logic/load_filament.cpp
           src/logic/no_command.cpp

src/logic/cut_filament.cpp

@@ -46,7 +46,7 @@ bool CutFilament::Step() {
             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:
+            case ErrorCode::FINDA_DIDNT_TRIGGER:
                 break;
             }
         }
@@ -54,10 +54,10 @@ bool CutFilament::Step() {
     case ProgressCode::SelectingFilamentSlot:
         if (mm::motion.QueueEmpty()) { // idler and selector finished their moves
             feed.Reset(true);
-            state = ProgressCode::FeedingToFINDA;
+            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
+    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
@@ -97,7 +97,7 @@ bool CutFilament::Step() {
         break;
     case ProgressCode::ReturningSelector:
         if (mm::motion.QueueEmpty()) { // selector returned to position, feed the filament back to FINDA
-            state = ProgressCode::FeedingToFINDA;
+            state = ProgressCode::FeedingToFinda;
             feed.Reset(true);
         }
         break;

src/logic/eject_filament.cpp

@@ -45,7 +45,7 @@ bool EjectFilament::Step() {
             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:
+            case ErrorCode::FINDA_DIDNT_TRIGGER:
                 break;
             }
         }

src/logic/error_codes.h

@@ -10,6 +10,7 @@ enum class ErrorCode : int_fast8_t {
     OK, ///< the operation finished OK
 
     /// Unload Filament related error codes
-    UNLOAD_FINDA_DIDNT_TRIGGER = -1, ///< FINDA didn't trigger while unloading filament - either there is something blocking the metal ball or a cable is broken/disconnected
+    FINDA_DIDNT_TRIGGER = -1, ///< FINDA didn't trigger while unloading filament - either there is something blocking the metal ball or a cable is broken/disconnected
     UNLOAD_ERROR2 = -2,
+
 };

src/logic/feed_to_bondtech.cpp

@@ -0,0 +1,59 @@
+#include "feed_to_bondtech.h"
+#include "../modules/buttons.h"
+#include "../modules/fsensor.h"
+#include "../modules/idler.h"
+#include "../modules/leds.h"
+#include "../modules/motion.h"
+#include "../modules/permanent_storage.h"
+
+namespace logic {
+
+namespace mm = modules::motion;
+namespace mfs = modules::fsensor;
+namespace mi = modules::idler;
+namespace ml = modules::leds;
+namespace mp = modules::permanent_storage;
+
+void FeedToBondtech::Reset(uint8_t maxRetries) {
+    state = EngagingIdler;
+    this->maxRetries = maxRetries;
+    mi::idler.Engage(0 /*active_extruder*/); // @@TODO
+}
+
+bool FeedToBondtech::Step() {
+    const uint16_t steps = mp::BowdenLength::get();
+
+    switch (state) {
+    case EngagingIdler:
+        if (mi::idler.Engaged()) {
+            state = PushingFilament;
+            ml::leds.SetMode(0, ml::Color::green, ml::blink0); //@@TODO active slot index
+            mm::motion.PlanMove(steps, 0, 0, 4500, 0, 0); //@@TODO constants - there was some strange acceleration sequence in the original FW,
+            // we can probably hand over some array of constants for hand-tuned acceleration + leverage some smoothing in the stepper as well
+        }
+        return false;
+    case PushingFilament:
+        if (mfs::fsensor.Pressed()) {
+            mm::motion.AbortPlannedMoves(); // stop pushing filament
+            state = DisengagingIdler;
+        } else if (mm::motion.StallGuard(mm::Pulley)) {
+            // stall guard occurred during movement - the filament got stuck
+            state = Failed; // @@TODO may be even report why it failed
+        } else if (mm::motion.QueueEmpty()) { // all moves have been finished and the fsensor didn't switch on
+            state = Failed;
+        }
+        return false;
+    case DisengagingIdler:
+        if (!mi::idler.Engaged()) {
+            state = OK;
+            ml::leds.SetMode(0, ml::Color::green, ml::on); //@@TODO active slot index
+        }
+        return false;
+    case OK:
+    case Failed:
+    default:
+        return true;
+    }
+}
+
+} // namespace logic

src/logic/feed_to_bondtech.h

@@ -0,0 +1,40 @@
+#pragma once
+#include <stdint.h>
+
+/// @brief Feed filament to Bondtech gears of the printer
+///
+/// Continuously feed filament until the printer detects the filament in its filament sensor
+
+namespace logic {
+
+struct FeedToBondtech {
+    enum {
+        EngagingIdler,
+        PushingFilament,
+        UnloadBackToPTFE,
+        DisengagingIdler,
+        OK,
+        Failed
+    };
+
+    inline FeedToBondtech()
+        : state(OK)
+        , maxRetries(1) {}
+
+    /// Restart the automaton
+    void Reset(uint8_t maxRetries);
+
+    /// @returns true if the state machine finished its job, false otherwise
+    bool Step();
+
+    /// This method may be used to check the result of the automaton
+    /// @returns OK if everything went OK and printer's filament sensor triggered
+    /// @returns Failed if the maximum feed length has been reached without the the printer's fsensor trigger being reported
+    inline uint8_t State() const { return state; }
+
+private:
+    uint8_t state;
+    uint8_t maxRetries;
+};
+
+} // namespace logic

src/logic/load_filament.cpp

@@ -1,25 +1,213 @@
 #include "load_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 {
 
 LoadFilament loadFilament;
 
 namespace mm = modules::motion;
+namespace mi = modules::idler;
+namespace ms = modules::selector;
+namespace mf = modules::finda;
+namespace ml = modules::leds;
 
 void LoadFilament::Reset(uint8_t param) {
     state = ProgressCode::EngagingIdler;
     error = ErrorCode::OK;
+    mi::idler.Engage(param); // @@TODO
 }
 
 bool LoadFilament::Step() {
     switch (state) {
+    case ProgressCode::EngagingIdler:
+        if (mi::idler.Engaged()) {
+            mm::motion.InitAxis(mm::Pulley);
+            state = ProgressCode::FeedingToFinda;
+            feed.Reset(true);
+        }
+        break;
+    case ProgressCode::FeedingToFinda:
+        if (feed.Step()) {
+            if (feed.State() == FeedToFinda::Failed) {
+                // @@TODO - try to repeat 6x - push/pull sequence - probably something to put into feed_to_finda as an option
+                state = ProgressCode::ERR1DisengagingIdler;
+                mi::idler.Disengage();
+                ml::leds.SetMode(0, ml::Color::red, ml::Mode::blink0); // signal loading error //@@TODO slot index
+            } else {
+                state = ProgressCode::FeedingToBondtech;
+                james.Reset(2);
+            }
+        }
+        break;
+    case ProgressCode::FeedingToBondtech:
+        if (james.Step()) {
+            switch (james.State()) {
+            case FeedToBondtech::Failed:
+
+            case FeedToBondtech::OK:
+                mm::motion.DisableAxis(mm::Pulley);
+                mi::idler.Disengage();
+                state = ProgressCode::DisengagingIdler;
+            }
+        }
+    case ProgressCode::DisengagingIdler:
+        if (!mi::idler.Engaged()) {
+            state = ProgressCode::OK;
+        }
+        break;
+    case ProgressCode::OK:
+        return true;
+    case ProgressCode::ERR1DisengagingIdler: // couldn't unload to FINDA
+        error = ErrorCode::FINDA_DIDNT_TRIGGER;
+        if (!mi::idler.Engaged()) {
+            state = ProgressCode::ERR1WaitingForUser;
+        }
+        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 tryAgain = modules::buttons::buttons.ButtonPressed(modules::buttons::Middle) /*|| command_tryAgain()*/;
+        bool userResolved = modules::buttons::buttons.ButtonPressed(modules::buttons::Right) /*|| command_userResolved()*/;
+        if (help) {
+            // try to manually load just a tiny bit - help the filament with the pulley
+            //@@TODO
+        } else if (tryAgain) {
+            // try again the whole sequence
+            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);
+            //                modules::leds::leds.SetMode(active_extruder, modules::leds::green, modules::leds::on);
+            //                mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants
+            state = ProgressCode::AvoidingGrind;
+        }
+        return false;
+    }
     }
     return false;
 }
 
+//! @brief Load filament through bowden
+//! @param disengageIdler
+//!  * true Disengage idler after movement
+//!  * false Do not disengage idler after movement
+//void load_filament_withSensor(bool disengageIdler)
+//{
+//    FilamentLoaded::set(active_extruder);
+//    motion_engage_idler();
+
+//    tmc2130_init_axis(AX_PUL, tmc2130_mode);
+
+//    set_pulley_dir_push();
+
+//    int _loadSteps = 0;
+//    int _endstop_hit = 0;
+
+//    // load filament until FINDA senses end of the filament, means correctly loaded into the selector
+//    // we can expect something like 570 steps to get in sensor
+//    do{
+//        do_pulley_step();
+//        _loadSteps++;
+//        delayMicroseconds(5500);
+//    } while (digitalRead(A1) == 0 && _loadSteps < 1500);
+
+//    // filament did not arrive at FINDA, let's try to correct that
+//    if (digitalRead(A1) == 0){
+//        for (int i = 6; i > 0; i--){
+//            if (digitalRead(A1) == 0){
+//                // attempt to correct
+//                set_pulley_dir_pull();
+//                for (int i = 200; i >= 0; i--){
+//                    do_pulley_step();
+//                    delayMicroseconds(1500);
+//                }
+
+//                set_pulley_dir_push();
+//                _loadSteps = 0;
+//                do{
+//                    do_pulley_step();
+//                    _loadSteps++;
+//                    delayMicroseconds(4000);
+//                    if (digitalRead(A1) == 1) _endstop_hit++;
+//                } while (_endstop_hit<100 && _loadSteps < 500);
+//            }
+//        }
+//    }
+
+//    // still not at FINDA, error on loading, let's wait for user input
+//    if (digitalRead(A1) == 0){
+//        bool _continue = false;
+//        bool _isOk = false;
+//        motion_disengage_idler();
+//        do{
+//            if (!_isOk){
+//                signal_load_failure();
+//            }else{
+//                signal_ok_after_load_failure();
+//            }
+
+//            switch (buttonPressed()){
+//                case Btn::left:
+//                    // just move filament little bit
+//                    motion_engage_idler();
+//                    set_pulley_dir_push();
+
+//                    for (int i = 0; i < 200; i++)
+//                    {
+//                        do_pulley_step();
+//                        delayMicroseconds(5500);
+//                    }
+//                    motion_disengage_idler();
+//                    break;
+//                case Btn::middle:
+//                    // check if everything is ok
+//                    motion_engage_idler();
+//                    _isOk = checkOk();
+//                    motion_disengage_idler();
+//                    break;
+//                case Btn::right:
+//                    // continue with loading
+//                    motion_engage_idler();
+//                    _isOk = checkOk();
+//                    motion_disengage_idler();
+
+//                    if (_isOk) //pridat do podminky flag ze od tiskarny prislo continue
+//                    {
+//                        _continue = true;
+//                    }
+//                    break;
+//                default:
+//                    break;
+//            }
+
+//        } while ( !_continue );
+
+//        motion_engage_idler();
+//        set_pulley_dir_push();
+//        _loadSteps = 0;
+//        do
+//        {
+//            do_pulley_step();
+//            _loadSteps++;
+//            delayMicroseconds(5500);
+//        } while (digitalRead(A1) == 0 && _loadSteps < 1500);
+//        // ?
+//    }
+//    else
+//    {
+//        // nothing
+//    }
+
+//    motion_feed_to_bondtech();
+
+//    tmc2130_disable_axis(AX_PUL, tmc2130_mode);
+//    if (disengageIdler) motion_disengage_idler();
+//    isFilamentLoaded = true;  // filament loaded
+
 } // namespace logic

src/logic/load_filament.h

@@ -1,7 +1,8 @@
 #pragma once
 #include <stdint.h>
 #include "command_base.h"
-#include "unload_to_finda.h"
+#include "feed_to_finda.h"
+#include "feed_to_bondtech.h"
 
 namespace logic {
 
@@ -19,6 +20,8 @@ public:
     bool Step() override;
 
 private:
+    FeedToFinda feed;
+    FeedToBondtech james; // bond ;)
 };
 
 extern LoadFilament loadFilament;

src/logic/progress_codes.h

@@ -8,19 +8,20 @@
 enum class ProgressCode : uint_fast8_t {
     OK = 0, ///< finished ok
 
-    /// Unload Filament related progress codes
     EngagingIdler,
-    UnloadingToFinda,
     DisengagingIdler,
+    UnloadingToFinda,
+    FeedingToFinda,
+    FeedingToBondtech,
     AvoidingGrind,
     FinishingMoves,
+
     ERR1DisengagingIdler,
     ERR1WaitingForUser,
 
     UnloadingFilament,
     LoadingFilament,
     SelectingFilamentSlot,
-    FeedingToFINDA,
     PreparingBlade,
     PushingFilament,
     PerformingCut,

src/logic/tool_change.cpp

@@ -43,7 +43,7 @@ bool ToolChange::Step() {
                 load.Reset(plannedSlot);
                 break;
             case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error?
-            case ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER:
+            case ErrorCode::FINDA_DIDNT_TRIGGER:
                 break;
             }
         }

src/logic/unload_filament.cpp

@@ -62,7 +62,7 @@ bool UnloadFilament::Step() {
         }
         return false;
     case ProgressCode::ERR1DisengagingIdler: // couldn't unload to FINDA
-        error = ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER;
+        error = ErrorCode::FINDA_DIDNT_TRIGGER;
         if (!mi::idler.Engaged()) {
             state = ProgressCode::ERR1WaitingForUser;
         }

src/main.cpp

@@ -10,6 +10,7 @@
 
 #include "modules/buttons.h"
 #include "modules/finda.h"
+#include "modules/fsensor.h"
 #include "modules/idler.h"
 #include "modules/leds.h"
 #include "modules/protocol.h"
@@ -205,6 +206,9 @@ void ProcessRequestMsg(const modules::protocol::RequestMsg &rq) {
     case mp::RequestMsgCodes::Unload:
         PlanCommand(rq);
         break;
+        //    case mp::RequestMsgCodes::SetVar: //@@TODO - this is where e.g. printer's fsensor gets updated
+        //        SetVar(rq);
+        //        break;
     default:
         // respond with an error message
         break;
@@ -252,6 +256,7 @@ void loop() {
     modules::buttons::buttons.Step(hal::adc::ReadADC(0));
     modules::leds::leds.Step(0);
     modules::finda::finda.Step(0);
+    modules::fsensor::fsensor.Step(0);
     modules::idler::idler.Step();
     modules::selector::selector.Step();
     currentCommand->Step();

src/modules/fsensor.cpp

@@ -0,0 +1,17 @@
+#include "fsensor.h"
+
+namespace modules {
+namespace fsensor {
+
+FSensor fsensor;
+
+void FSensor::Step(uint16_t time) {
+    debounce::Debouncer::Step(time, reportedFSensorState);
+}
+
+void FSensor::ProcessMessage(bool on) {
+    reportedFSensorState = on;
+}
+
+} // namespace finda
+} // namespace modules

src/modules/fsensor.h

@@ -0,0 +1,30 @@
+#pragma once
+#include <stdint.h>
+#include "debouncer.h"
+
+/// External module - model of printer's fsensor
+
+namespace modules {
+namespace fsensor {
+
+/// the debouncer is probably not necessary, but it has all the necessary functionality for modelling of the fsensor
+
+class FSensor : protected debounce::Debouncer {
+public:
+    inline FSensor()
+        : debounce::Debouncer(debounce) {};
+    void Step(uint16_t time);
+    using debounce::Debouncer::Pressed;
+
+    void ProcessMessage(bool on);
+
+private:
+    /// time interval for debouncing @@TODO specify units
+    constexpr static const uint16_t debounce = 10;
+    bool reportedFSensorState; ///< reported state that came from the printer via a communication message
+};
+
+extern FSensor fsensor;
+
+} // namespace finda
+} // namespace modules

src/modules/idler.h

@@ -17,9 +17,9 @@ public:
     };
 
     inline Idler()
-        : plannedEngage(false)
+        : state(Ready)
+        , plannedEngage(false)
         , plannedSlot(0)
-        , state(Ready)
         , currentSlot(0)
         , currentlyEngaged(false) {}
 

src/modules/motion.h

@@ -68,6 +68,12 @@ public:
     /// Disable axis motor
     void DisableAxis(Axis axis) {}
 
+    /// @returns true if a stall guard event occurred recently on the axis
+    bool StallGuard(Axis axis) { return false; }
+
+    /// clear stall guard flag reported on an axis
+    void ClearStallGuardFlag(Axis axis) {}
+
     /// Enqueue move of a specific motor/axis into planner buffer
     /// @param pulley, idler, selector - target coords
     void PlanMove(uint16_t pulley, uint16_t idler, uint16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed);

src/modules/selector.h

@@ -18,8 +18,8 @@ public:
 
     inline Selector()
         : state(Ready)
-        , currentSlot(0)
+        , plannedSlot(0)
-        , plannedSlot(0) {}
+        , currentSlot(0) {}
 
     // public operations on the selector