From 108bb18823df8093888111a047d3e2894d4208a4 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Wed, 9 Jun 2021 09:30:37 +0200
Subject: [PATCH] Add Idler module

---
 src/modules/idler.cpp | 59 ++++++++++++++++++++++++++++++++++++++++
 src/modules/idler.h   | 62 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 121 insertions(+)
 create mode 100644 src/modules/idler.cpp
 create mode 100644 src/modules/idler.h

diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
new file mode 100644
index 0000000..bad83da
--- /dev/null
+++ b/src/modules/idler.cpp
@@ -0,0 +1,59 @@
+#include "idler.h"
+#include "buttons.h"
+#include "leds.h"
+#include "motion.h"
+#include "permanent_storage.h"
+
+namespace modules {
+namespace idler {
+
+namespace mm = modules::motion;
+
+bool Idler::Disengage() {
+    if (state == Moving)
+        return false;
+    plannedEngage = false;
+    // plan move to idle position
+    // mm::motion.PlanMove(0, idle_position, 0, 1000, 0, 0); // @@TODO
+    state = Moving;
+    return true;
+}
+
+bool Idler::Engage(uint8_t slot) {
+    if (state == Moving)
+        return false;
+    plannedSlot = slot;
+    plannedEngage = true;
+    // mm::motion.PlanMove(0, slotPositions[slot], 0, 1000, 0, 0); // @@TODO
+    state = Moving;
+    return true;
+}
+
+bool Idler::Home() {
+    if (state == Moving)
+        return false;
+    plannedEngage = false;
+    mm::motion.Home(mm::Idler, false);
+    return true;
+}
+
+bool Idler::Step() {
+    switch (state) {
+    case Moving:
+        if (mm::motion.QueueEmpty()) {
+            // move finished
+            state = Ready;
+        }
+        return false;
+    case Ready:
+        currentlyEngaged = plannedEngage;
+        currentSlot = plannedSlot;
+        return true;
+    case Failed:
+    default:
+        return true;
+    }
+}
+
+} // namespace idler
+} // namespace modules
diff --git a/src/modules/idler.h b/src/modules/idler.h
new file mode 100644
index 0000000..dc76d65
--- /dev/null
+++ b/src/modules/idler.h
@@ -0,0 +1,62 @@
+#pragma once
+#include <stdint.h>
+
+/// Idler model
+/// Handles asynchronnous Engaging / Disengaging operations
+/// Keeps track of idler's current state
+
+namespace modules {
+namespace idler {
+
+class Idler {
+public:
+    enum {
+        Ready = 0,
+        Moving,
+        Failed
+    };
+
+    // public operations on the idler
+
+    /// @retuns false in case an operation is already underway
+    bool Engage(uint8_t slot);
+    /// @retuns false in case an operation is already underway
+    bool Disengage();
+    /// @retuns false in case an operation is already underway
+    bool Home();
+
+    /// @returns true if the idler is ready to accept new commands (i.e. it has finished the last operation)
+    bool Step();
+
+    /// @returns the current state of idler - engaged / disengaged
+    /// 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; }
+    inline uint8_t Slot() const { return currentSlot; }
+
+private:
+    constexpr static const uint16_t slotPositions[5] = { 1, 2, 3, 4, 5 }; // @@TODO
+
+    /// internal state of the automaton
+    uint8_t state;
+
+    /// direction of travel - engage/disengage
+    bool plannedEngage;
+    uint8_t plannedSlot;
+
+    /// current state
+    uint8_t currentSlot;
+    bool currentlyEngaged;
+
+    inline Idler()
+        : plannedEngage(false)
+        , plannedSlot(0)
+        , state(Ready)
+        , currentSlot(0)
+        , currentlyEngaged(false) {}
+};
+
+extern Idler idler;
+
+} // namespace idler
+} // namespace modules