From 324fb92fd53221236f727fd238f720cdabdbac4f Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Tue, 13 Jul 2021 19:21:13 +0200
Subject: [PATCH] Motion: implement timer multiplexing

---
 src/config/config.h                       |  4 +++
 src/modules/motion.cpp                    | 24 ++++++++++++-
 src/modules/motion.h                      |  1 +
 tests/unit/modules/motion/test_motion.cpp | 43 +++++++++++++++++++++++
 4 files changed, 71 insertions(+), 1 deletion(-)

diff --git a/src/config/config.h b/src/config/config.h
index b1c247f..3875d94 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -47,6 +47,10 @@ static constexpr uint8_t blockBufferSize = 2;
 /// Step timer frequency divider (F = F_CPU / divider)
 static constexpr uint8_t stepTimerFrequencyDivider = 8;
 
+/// Smallest stepping ISR scheduling slice (T = F_CPU / divider * quantum)
+/// 16 = 8us (25us is the max frequency interval per maxStepFrequency)
+static constexpr uint8_t stepTimerQuantum = 16;
+
 /// Idler configuration
 static constexpr AxisConfig idler = {
     .dirOn = true,
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 1380be7..26386fd 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -56,7 +56,29 @@ void Motion::AbortPlannedMoves() {
 }
 
 st_timer_t Motion::Step() {
-    return 0;
+    st_timer_t timers[NUM_AXIS];
+
+    // step and calculate interval for each new move
+    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+        timers[i] = axisData[i].residual;
+        if (timers[i] <= config::stepTimerQuantum) {
+            timers[i] += axisData[i].ctrl.Step(axisParams[i].params);
+        }
+    }
+
+    // plan next closest interval
+    st_timer_t next = timers[0];
+    for (uint8_t i = 1; i != NUM_AXIS; ++i) {
+        if (timers[i] && (!next || timers[i] < next))
+            next = timers[i];
+    }
+
+    // update residuals
+    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+        axisData[i].residual = (timers[i] ? timers[i] - next : 0);
+    }
+
+    return next;
 }
 
 void ISR() {}
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 6dc99db..301a4c9 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -168,6 +168,7 @@ private:
         TMC2130 drv; ///< Motor driver
         pulse_gen::PulseGen ctrl; ///< Motor controller
         bool enabled; ///< Axis enabled
+        st_timer_t residual; ///< Axis timer residual
     };
 
     /// Helper to initialize AxisData members
diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp
index c73c246..1702651 100644
--- a/tests/unit/modules/motion/test_motion.cpp
+++ b/tests/unit/modules/motion/test_motion.cpp
@@ -125,3 +125,46 @@ TEST_CASE("motion::triple_move", "[motion]") {
     REQUIRE(motion.Position(Selector) == 20);
     REQUIRE(motion.Position(Pulley) == 30);
 }
+
+TEST_CASE("motion::dual_move_ramp", "[motion]") {
+    // TODO: output ramps still to be checked
+    const int idlerSteps = 100;
+    const int selectorSteps = 80;
+    const int maxFeedRate = 1000;
+
+    for (int accel = 2000; accel <= 50000; accel *= 2) {
+        REQUIRE(motion.QueueEmpty());
+
+        // first axis using nominal values
+        motion.SetPosition(Idler, 0);
+        motion.SetAcceleration(Idler, accel);
+        motion.PlanMoveTo(Idler, idlerSteps, maxFeedRate);
+
+        // second axis finishes slightly sooner at triple acceleration to maximize the
+        // aliasing effects
+        motion.SetPosition(Selector, 0);
+        motion.SetAcceleration(Selector, accel * 3);
+        motion.PlanMoveTo(Selector, selectorSteps, maxFeedRate);
+
+        // step and output time, interval and positions
+        unsigned long ts = 0;
+        st_timer_t next;
+        do {
+            next = motion.Step();
+            pos_t pos_idler = motion.CurPosition(Idler);
+            pos_t pos_selector = motion.CurPosition(Selector);
+
+            printf("%lu %u %d %d\n", ts, next, pos_idler, pos_selector);
+
+            ts += next;
+        } while (next);
+        printf("\n\n");
+
+        // check queue status
+        REQUIRE(motion.QueueEmpty());
+
+        // check final position
+        REQUIRE(motion.Position(Idler) == idlerSteps);
+        REQUIRE(motion.Position(Selector) == selectorSteps);
+    }
+}