Introduce manual operation of MMU

This PR brings the option to move the selector directly using
buttons of the MMU - obviously while the MMU is idle and no
filament is stuck in the selector.

Left/Right buttons move the selector Left/Right.

Middle button performs a LoadFilament (into the MMU) on the active slot.

With this PR a change of LoadFilament behavior is also introduced.
Now, LoadFilament spins the Pulley for infinite time while waiting
for either FINDA trigger and/or a button pressed.

src/hal/circular_buffer.h

@@ -55,6 +55,11 @@ public:
         return mask(head);
     }
 
+    /// @returns number of elements in the buffer
+    inline index_t count() const {
+        return 0; // @@TODO
+    }
+
 protected:
     index_t tail; ///< cursor of the element to read (pop/extract) from the buffer
     index_t head; ///< cursor of the empty spot or element insertion (write)

src/logic/feed_to_finda.cpp

@@ -29,19 +29,27 @@ bool FeedToFinda::Step() {
         if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
             dbg_logic_P(PSTR("Feed to Finda --> Idler engaged"));
             dbg_logic_fP(PSTR("Pulley start steps %u"), mpu::pulley.CurrentPositionPulley_mm());
-            state = PushingFilament;
             mpu::pulley.InitAxis();
             // @@TODO this may never happen as load filament always assumes the filament is at least at the pulley
             //            if (mg::globals.FilamentLoaded() == mg::FilamentLoadState::NotLoaded) { // feed slowly filament to PTFE
             //                mpu::pulley.PlanMove(config::filamentMinLoadedToMMU, config::pulleySlowFeedrate);
             //            }
+
             mpu::pulley.PlanMove(config::maximumFeedToFinda, config::pulleySlowFeedrate);
+            if (feedPhaseLimited) {
+                state = PushingFilament;
+            } else {
+                state = PushingFilamentUnlimited;
+                // in unlimited move we plan 2 moves at once to make the move "seamless"
+                // one move has already been planned above, this is the second one
+                mpu::pulley.PlanMove(config::maximumFeedToFinda, config::pulleySlowFeedrate);
+            }
             mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
             mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input
         }
         return false;
     case PushingFilament: {
-        if (mf::finda.Pressed() || (feedPhaseLimited && mui::userInput.AnyEvent())) {
+        if (mf::finda.Pressed()) {
             mm::motion.AbortPlannedMoves(haltAtEnd); // stop pushing filament
             // FINDA triggered - that means it works and detected the filament tip
             mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
@@ -55,6 +63,22 @@ bool FeedToFinda::Step() {
         }
     }
         return false;
+    case PushingFilamentUnlimited: {
+        // FINDA triggered or the user pressed a button -> stop move, consider the operation as finished ok
+        if (mf::finda.Pressed() || mui::userInput.AnyEvent()) {
+            mm::motion.AbortPlannedMoves(haltAtEnd); // stop pushing filament
+            // FINDA triggered - that means it works and detected the filament tip
+            mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InSelector);
+            dbg_logic_P(PSTR("Feed to Finda --> Idler disengaged"));
+            dbg_logic_fP(PSTR("Pulley end steps %u"), mpu::pulley.CurrentPositionPulley_mm());
+            state = OK;
+            return true; // return immediately to allow for a seamless planning of another move (like feeding to bondtech)
+        } else if (mm::motion.PlannedMoves(mm::Pulley) < 2) {
+            // plan another move to make the illusion of unlimited moves
+            mpu::pulley.PlanMove(config::maximumFeedToFinda, config::pulleySlowFeedrate);
+        }
+    }
+        return false;
     case OK:
         dbg_logic_P(PSTR("Feed to FINDA OK"));
         return true;

src/logic/feed_to_finda.h

@@ -15,6 +15,7 @@ struct FeedToFinda {
     enum {
         EngagingIdler,
         PushingFilament,
+        PushingFilamentUnlimited,
         UnloadBackToPTFE,
         DisengagingIdler,
         OK,

src/logic/load_filament.cpp

@@ -21,13 +21,22 @@ void LoadFilament::Reset(uint8_t param) {
     }
     dbg_logic_P(PSTR("Load Filament"));
     mg::globals.SetFilamentLoaded(param, mg::FilamentLoadState::AtPulley); // still at pulley, haven't moved yet
-    Reset2();
+    Reset2(false);
 }
 
-void logic::LoadFilament::Reset2() {
+void LoadFilament::ResetUnlimited(uint8_t param) {
+    if (!CheckToolIndex(param)) {
+        return;
+    }
+    dbg_logic_P(PSTR("Load Filament"));
+    mg::globals.SetFilamentLoaded(param, mg::FilamentLoadState::AtPulley); // still at pulley, haven't moved yet
+    Reset2(true);
+}
+
+void logic::LoadFilament::Reset2(bool unlimited) {
     state = ProgressCode::FeedingToFinda;
     error = ErrorCode::RUNNING;
-    feed.Reset(true, true);
+    feed.Reset(unlimited, true);
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off);
 }
 

src/logic/load_filament.h

@@ -17,12 +17,16 @@ public:
     /// @param param index of filament slot to load
     void Reset(uint8_t param) override;
 
+    /// Restart the automaton for unlimited rotation of the Pulley
+    /// @param param index of filament slot to load
+    void ResetUnlimited(uint8_t param);
+
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;
 
 private:
     void GoToRetractingFromFinda();
-    void Reset2();
+    void Reset2(bool unlimited);
 
     /// Common code for a correct completion of UnloadFilament
     void FinishedCorrectly();

src/logic/move_selector.cpp

@@ -0,0 +1,40 @@
+/// @file
+#include "move_selector.h"
+#include "../modules/globals.h"
+#include "../modules/selector.h"
+#include "../debug.h"
+
+namespace logic {
+
+MoveSelector home;
+
+void MoveSelector::Reset(uint8_t param) {
+    state = ProgressCode::MovingSelector;
+
+    if (ms::selector.MoveToSlot(param) != ms::Selector::OperationResult::Refused) {
+        // operation accepted
+        error = ErrorCode::RUNNING;
+    } else {
+        error = ErrorCode::HOMING_SELECTOR_FAILED; // @@TODO
+    }
+}
+
+bool MoveSelector::StepInner() {
+    switch (state) {
+    case ProgressCode::MovingSelector:
+        if (ms::selector.State() == ms::selector.Ready) {
+            state = ProgressCode::OK;
+            error = ErrorCode::OK;
+        }
+        break;
+    case ProgressCode::OK:
+        return true;
+    default: // we got into an unhandled state, better report it
+        state = ProgressCode::ERRInternal;
+        error = ErrorCode::INTERNAL;
+        return true;
+    }
+    return false;
+}
+
+} // namespace logic

src/logic/move_selector.h

@@ -0,0 +1,32 @@
+/// @file home.h
+#pragma once
+#include <stdint.h>
+#include "command_base.h"
+
+namespace logic {
+
+/// @brief  A high-level command state machine - wrapps the rehoming procedure to be used from a printer
+///
+/// The Move-Selector operation consists of:
+/// - issue a new coordinate to the Selector
+///
+/// Beware - Selector will NOT perform the move if filament sensor state is not in the right state
+/// This high-level command is just a way to invoke moving the Selector from the printer
+/// and/or from the MMU's buttons while all safety measures are kept.
+class MoveSelector : public CommandBase {
+public:
+    inline MoveSelector()
+        : CommandBase() {}
+
+    /// Restart the automaton
+    /// @param param target selector slot
+    void Reset(uint8_t param) override;
+
+    /// @returns true if the state machine finished its job, false otherwise
+    bool StepInner() override;
+};
+
+/// The one and only instance of  MoveSelector state machine in the FW
+extern MoveSelector moveSelector;
+
+} // namespace logic

src/logic/progress_codes.h

@@ -36,7 +36,8 @@ enum class ProgressCode : uint_fast8_t {
     EjectingFilament, // P24
     RetractingFromFinda, // P25
 
-    Homing,
+    Homing, // P26
+    MovingSelector, // P27
 
     Empty = 0xff // dummy empty state
 };

src/main.cpp

@@ -30,6 +30,7 @@
 #include "logic/eject_filament.h"
 #include "logic/home.h"
 #include "logic/load_filament.h"
+#include "logic/move_selector.h"
 #include "logic/no_command.h"
 #include "logic/set_mode.h"
 #include "logic/tool_change.h"
@@ -42,7 +43,7 @@
 static mp::Protocol protocol;
 
 /// A command that resulted in the currently on-going operation
-logic::CommandBase *currentCommand = &logic::noCommand;
+static logic::CommandBase *currentCommand = &logic::noCommand;
 
 /// Remember the request message that started the currently running command
 /// For the start we report "Reset finished" which in fact corresponds with the MMU state pretty closely
@@ -50,7 +51,9 @@ logic::CommandBase *currentCommand = &logic::noCommand;
 /// And, since the default startup command is the noCommand, which always returns "Finished"
 /// the implementation is clean and straightforward - the response to the first Q0 messages
 /// will look like "X0 F" until a command (T, L, U ...) has been issued.
-mp::RequestMsg currentCommandRq(mp::RequestMsgCodes::Reset, 0);
+static mp::RequestMsg currentCommandRq(mp::RequestMsgCodes::Reset, 0);
+
+static uint16_t lastCommandProcessedMs = 0;
 
 /// One-time setup of HW and SW components
 /// Called before entering the loop() function
@@ -205,6 +208,7 @@ void ReportRunningCommand() {
         break;
     case ErrorCode::OK:
         commandStatus = mp::ResponseMsgParamCodes::Finished;
+        lastCommandProcessedMs = mt::timebase.Millis();
         break;
     default:
         commandStatus = mp::ResponseMsgParamCodes::Error;
@@ -322,6 +326,41 @@ void Panic(ErrorCode ec) {
     currentCommand->Panic(ec);
 }
 
+bool CheckManualOperation() {
+    if (currentCommand == &logic::noCommand
+        && mg::globals.FilamentLoaded() <= mg::FilamentLoadState::AtPulley
+        && lastCommandProcessedMs + 5000 < mt::timebase.Millis()) {
+        lastCommandProcessedMs = mt::timebase.Millis() - 5000; // @@TODO prevent future overflows - must be called every time
+        if (mui::userInput.AnyEvent()) {
+            switch (mui::userInput.ConsumeEvent()) {
+            case mui::Event::Left:
+                // move selector left if possible
+                if (mg::globals.ActiveSlot() > 0) {
+                    currentCommand = &logic::moveSelector;
+                    currentCommand->Reset(mg::globals.ActiveSlot() - 1);
+                }
+                break;
+            case mui::Event::Middle:
+                // plan load
+                if (mg::globals.ActiveSlot() < config::toolCount) { // do we have a meaningful selector position?
+                    logic::loadFilament.ResetUnlimited(mg::globals.ActiveSlot());
+                    currentCommand = &logic::loadFilament;
+                }
+                break;
+            case mui::Event::Right:
+                // move selector right if possible (including the park position)
+                if (mg::globals.ActiveSlot() < config::toolCount) {
+                    currentCommand = &logic::moveSelector;
+                    currentCommand->Reset(mg::globals.ActiveSlot() + 1); // we allow also the park position
+                }
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
 /// Main loop of the firmware
 /// Proposed architecture
 ///   checkMsgs();
@@ -337,9 +376,12 @@ void Panic(ErrorCode ec) {
 /// The idea behind the Step* routines is to keep each automaton non-blocking allowing for some “concurrency”.
 /// Some FW components will leverage ISR to do their stuff (UART, motor stepping?, etc.)
 void loop() {
+    CheckManualOperation();
+
     if (CheckMsgs()) {
         ProcessRequestMsg(protocol.GetRequestMsg());
     }
+
     mb::buttons.Step();
     ml::leds.Step();
     mf::finda.Step();

src/modules/motion.h

@@ -313,6 +313,11 @@ public:
     bool QueueEmpty(Axis axis) const;
 #endif
 
+    /// @returns number of planned moves on an axis
+    uint8_t PlannedMoves(Axis axis) const {
+        return axisData[axis].ctrl.QueueEmpty();
+    }
+
     /// @returns false if new moves can still be planned for one axis
     /// @param axis axis requested
     bool Full(Axis axis) const { return axisData[axis].ctrl.Full(); }

src/modules/pulse_gen.h

@@ -26,16 +26,16 @@ public:
     PulseGen(steps_t max_jerk, steps_t acceleration);
 
     /// @returns the jerk for the axis
-    steps_t Jerk() const { return max_jerk; };
+    inline steps_t Jerk() const { return max_jerk; };
 
     /// Set maximum jerk for the axis
-    void SetJerk(steps_t max_jerk) { this->max_jerk = max_jerk; };
+    inline void SetJerk(steps_t max_jerk) { this->max_jerk = max_jerk; };
 
     /// @returns the acceleration for the axis
-    steps_t Acceleration() const { return acceleration; };
+    inline steps_t Acceleration() const { return acceleration; };
 
     /// Set acceleration for the axis
-    void SetAcceleration(steps_t accel) { acceleration = accel; }
+    inline void SetAcceleration(steps_t accel) { acceleration = accel; }
 
     /// Enqueue a single move in steps starting and ending at zero speed with maximum
     /// feedrate. Moves can only be enqueued if the axis is not Full().
@@ -61,17 +61,19 @@ public:
     /// Set the position of the axis
     /// Should only be called when the queue is empty.
     /// @param x position to set
-    void SetPosition(pos_t x) { position = x; }
+    inline void SetPosition(pos_t x) { position = x; }
 
     /// Fetch the target rate of the last planned segment, or the current effective rate
     /// when the move has been aborted.
-    steps_t Rate() const { return last_rate; }
+    inline steps_t Rate() const { return last_rate; }
 
     /// @returns true if all planned moves have been finished
-    bool QueueEmpty() const { return block_index.empty(); }
+    inline bool QueueEmpty() const { return block_index.empty(); }
 
     /// @returns false if new moves can still be planned
-    bool Full() const { return block_index.full(); }
+    inline bool Full() const { return block_index.full(); }
+
+    inline uint8_t PlannedMoves() const { return block_index.count(); }
 
     /// Single-step the axis
     /// @returns the interval for the next tick