Merge pull request #47 from wavexx/motion_wip

WIP: Motion API
2021-07-14 08:26:49 +02:00 · 2021-07-14 08:26:49 +02:00 · 52e3c3375a
parent c81072d1d5 4160d019be
commit 52e3c3375a
42 changed files with 2172 additions and 174 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -143,12 +143,16 @@ if(CMAKE_CROSSCOMPILING)

  # produce ASM listing
  add_custom_command(
-    TARGET firmware POST_BUILD COMMAND ${CMAKE_OBJDUMP} -CSd firmware > firmware.asm
+    TARGET firmware
+    POST_BUILD
+    COMMAND ${CMAKE_OBJDUMP} -CSd firmware > firmware.asm
    )

  # inform about the firmware's size in terminal
  add_custom_command(
-    TARGET firmware POST_BUILD COMMAND ${CMAKE_SIZE_UTIL} -C --mcu=atmega32u4 firmware
+    TARGET firmware
+    POST_BUILD
+    COMMAND ${CMAKE_SIZE_UTIL} -C --mcu=atmega32u4 firmware
    )
  report_size(firmware)

--- a/src/cmath.h
+++ b/src/cmath.h
@ -0,0 +1,28 @@
+// Provide an uniform interface for basic math functions between AVR libc and newer
+// standards that support <cmath>
+#pragma once
+
+#ifndef __AVR__
+#include <cmath>
+#else
+
+// AVR libc doesn't support cmath
+#include <math.h>
+
+// Use builtin functions for min/max/abs
+template <typename T>
+static inline const T min(T a, T b) {
+    return __builtin_min((a, b));
+}
+
+template <typename T>
+static inline const T max(T a, T b) {
+    return __builtin_max((a, b));
+}
+
+template <typename T>
+static inline const T abs(T n) {
+    return __builtin_abs((n));
+}
+
+#endif
--- a/src/config/axis.h
+++ b/src/config/axis.h
@ -0,0 +1,18 @@
+#pragma once
+#include <stdint.h>
+
+namespace config {
+
+/// Axis configuration data
+struct AxisConfig {
+    bool dirOn; ///< direction ON state (for inversion)
+    uint8_t uSteps; ///< microstepping [1-32]
+    bool vSense; ///< vSense scaling
+    uint8_t iRun; ///< running current
+    uint8_t iHold; ///< holding current
+    uint16_t accel; ///< Acceleration (unit/s^2)
+    uint16_t jerk; ///< Jerk (unit/s)
+    bool stealth; ///< Default to Stealth mode
+};
+
+} // namespace config
--- a/src/config/config.h
+++ b/src/config/config.h
@ -1,5 +1,6 @@
 #pragma once
 #include <stdint.h>
+#include "axis.h"

 /// Wrangler for assorted compile-time configuration and constants.
 namespace config {
@ -26,4 +27,64 @@ static constexpr const uint16_t buttonsDebounceMs = 100;
 static constexpr const uint16_t buttonADCLimits[buttonCount][2] = { { 0, 50 }, { 80, 100 }, { 160, 180 } };
 static constexpr const uint8_t buttonsADCIndex = 5; ///< ADC index of buttons input

+/// Maximum microstepping resolution. This defines the effective unit of
+/// the step intevals on the motion API, independently of the selected
+/// microstepping interval.
+static constexpr uint8_t uStepMaxRes = 32;
+
+/// Do not plan moves equal or shorter than the requested steps
+static constexpr uint8_t dropSegments = 0;
+
+/// Max step frequency 40KHz
+static constexpr uint16_t maxStepFrequency = 40000;
+
+/// Minimum stepping rate 120Hz
+static constexpr uint16_t minStepRate = 120;
+
+/// Size for the motion planner block buffer size
+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,
+    .uSteps = 16,
+    .vSense = false,
+    .iRun = 20,
+    .iHold = 20,
+    .accel = 100,
+    .jerk = 10,
+    .stealth = false,
+};
+
+/// Pulley configuration
+static constexpr AxisConfig pulley = {
+    .dirOn = true,
+    .uSteps = 16,
+    .vSense = false,
+    .iRun = 20,
+    .iHold = 20,
+    .accel = 100,
+    .jerk = 10,
+    .stealth = false,
+};
+
+/// Selector configuration
+static constexpr AxisConfig selector = {
+    .dirOn = true,
+    .uSteps = 16,
+    .vSense = false,
+    .iRun = 20,
+    .iHold = 20,
+    .accel = 100,
+    .jerk = 10,
+    .stealth = false
+};
+
 } // namespace config
--- a/src/hal/CMakeLists.txt
+++ b/src/hal/CMakeLists.txt
@ -1,9 +1,3 @@
 target_sources(
-  firmware
-  PRIVATE
-  avr/cpu.cpp
-  avr/usart.cpp
-  avr/shr16.cpp
-  avr/eeprom.cpp
-  adc.cpp
-)
+  firmware PRIVATE avr/cpu.cpp avr/usart.cpp avr/shr16.cpp avr/eeprom.cpp avr/tmc2130.cpp adc.cpp
+  )
--- a/src/hal/avr/tmc2130.cpp
+++ b/src/hal/avr/tmc2130.cpp
@ -0,0 +1,13 @@
+#include "../tmc2130.h"
+
+namespace hal {
+namespace tmc2130 {
+
+TMC2130::TMC2130(const MotorParams &params,
+    const MotorCurrents &currents,
+    MotorMode mode) {
+    // TODO
+}
+
+} // namespace tmc2130
+} // namespace hal
--- a/src/hal/cpu.h
+++ b/src/hal/cpu.h
@ -5,6 +5,11 @@
 namespace hal {
 namespace cpu {

+#ifndef F_CPU
+/// Main clock frequency
+#define F_CPU (16000000ul)
+#endif
+
 /// CPU init routines (not really necessary for the AVR)
 void Init();

--- a/src/logic/eject_filament.cpp
+++ b/src/logic/eject_filament.cpp
@ -50,7 +50,7 @@ bool EjectFilament::Step() {
        if (mm::motion.QueueEmpty()) { // selector parked aside
            state = ProgressCode::EjectingFilament;
            mm::motion.InitAxis(mm::Pulley);
-            mm::motion.PlanMove(ejectSteps, 0, 0, 1500, 0, 0);
+            mm::motion.PlanMove(mm::Pulley, ejectSteps, 1500);
        }
        break;
    case ProgressCode::EjectingFilament:
@ -61,7 +61,7 @@ bool EjectFilament::Step() {
        break;
    case ProgressCode::DisengagingIdler:
        if (!mi::idler.Engaged()) { // idler disengaged
-            mm::motion.DisableAxis(mm::Pulley);
+            mm::motion.Disable(mm::Pulley);
            state = ProgressCode::OK;
        }
        break;
--- a/src/logic/feed_to_bondtech.cpp
+++ b/src/logic/feed_to_bondtech.cpp
@ -30,7 +30,7 @@ bool FeedToBondtech::Step() {
        if (mi::idler.Engaged()) {
            state = PushingFilament;
            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
-            mm::motion.PlanMove(steps, 0, 0, 4500, 0, 0); //@@TODO constants - there was some strange acceleration sequence in the original FW,
+            mm::motion.PlanMove(mm::Pulley, steps, 4500); //@@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;
--- a/src/logic/feed_to_finda.cpp
+++ b/src/logic/feed_to_finda.cpp
@ -32,7 +32,7 @@ bool FeedToFinda::Step() {
        if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
            state = PushingFilament;
            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
-            mm::motion.PlanMove(feedPhaseLimited ? 1500 : 32767, 0, 0, 4000, 0, 0); //@@TODO constants
+            mm::motion.PlanMove(mm::Pulley, feedPhaseLimited ? 1500 : 32767, 4000); //@@TODO constants
            mu::userInput.Clear(); // remove all buffered events if any just before we wait for some input
        }
        return false;
@ -41,7 +41,7 @@ bool FeedToFinda::Step() {
            mm::motion.AbortPlannedMoves(); // stop pushing filament
            // FINDA triggered - that means it works and detected the filament tip
            state = UnloadBackToPTFE;
-            mm::motion.PlanMove(-600, 0, 0, 4000, 0, 0); //@@TODO constants
+            mm::motion.PlanMove(mm::Pulley, -600, 4000); //@@TODO constants
        } else if (mm::motion.QueueEmpty()) { // all moves have been finished and FINDA didn't switch on
            state = Failed;
            // @@TODO - shall we disengage the idler?
--- a/src/logic/load_filament.cpp
+++ b/src/logic/load_filament.cpp
@ -59,7 +59,7 @@ bool LoadFilament::Step() {
            case FeedToBondtech::Failed:

            case FeedToBondtech::OK:
-                mm::motion.DisableAxis(mm::Pulley);
+                mm::motion.Disable(mm::Pulley);
                mi::idler.Disengage();
                state = ProgressCode::DisengagingIdler;
            }
--- a/src/logic/unload_filament.cpp
+++ b/src/logic/unload_filament.cpp
@ -63,7 +63,7 @@ bool UnloadFilament::Step() {
    case ProgressCode::FinishingMoves:
        if (mm::motion.QueueEmpty()) {
            state = ProgressCode::OK;
-            mm::motion.DisableAxis(mm::Pulley);
+            mm::motion.Disable(mm::Pulley);
            mg::globals.SetFilamentLoaded(false); // filament unloaded
            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
        }
--- a/src/modules/CMakeLists.txt
+++ b/src/modules/CMakeLists.txt
@ -1,17 +1,17 @@
 target_sources(
  firmware
-  PRIVATE
-  protocol.cpp
-  buttons.cpp
-  debouncer.cpp
-  finda.cpp
-  fsensor.cpp
-  globals.cpp
-  idler.cpp
-  leds.cpp
-  motion.cpp
-  permanent_storage.cpp
-  selector.cpp
-  timebase.cpp
-  user_input.cpp
-)
+  PRIVATE protocol.cpp
+          buttons.cpp
+          debouncer.cpp
+          finda.cpp
+          fsensor.cpp
+          globals.cpp
+          idler.cpp
+          leds.cpp
+          motion.cpp
+          permanent_storage.cpp
+          selector.cpp
+          timebase.cpp
+          user_input.cpp
+          pulse_gen.cpp
+  )
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@ -22,7 +22,7 @@ bool Idler::Disengage() {

    mm::motion.InitAxis(mm::Idler);
    // plan move to idle position
-    mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[IdleSlotIndex()] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO
+    mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[IdleSlotIndex()] - mm::motion.Position(mm::Idler), 1000); // @@TODO
    state = Moving;
    return true;
 }
@ -38,7 +38,7 @@ bool Idler::Engage(uint8_t slot) {
        return true;

    mm::motion.InitAxis(mm::Idler);
-    mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[slot] - mm::motion.CurrentPos(mm::Idler), 1000); // @@TODO
+    mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[slot] - mm::motion.Position(mm::Idler), 1000); // @@TODO
    state = Moving;
    return true;
 }
@ -66,7 +66,7 @@ bool Idler::Step() {
        currentSlot = plannedSlot;

        if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary)
-            mm::motion.DisableAxis(mm::Idler);
+            mm::motion.Disable(mm::Idler);

        return true;
    case Failed:
--- a/src/modules/math.h
+++ b/src/modules/math.h
@ -0,0 +1,78 @@
+#pragma once
+#include "../config/config.h"
+
+namespace modules {
+
+/// Specialized math operations
+namespace math {
+
+/// (intIn1 * intIn2) >> 8
+static inline uint16_t mulU8X16toH16(const uint8_t charIn1, const uint16_t intIn2) {
+    uint16_t intRes;
+#if !defined(__AVR__) || defined(NO_ASM)
+    intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8;
+#else
+    asm volatile(
+        "clr r26        \n\t"
+        "mul %A1, %B2   \n\t"
+        "movw %A0, r0   \n\t"
+        "mul %A1, %A2   \n\t"
+        "add %A0, r1    \n\t"
+        "adc %B0, r26   \n\t"
+        "lsr r0         \n\t"
+        "adc %A0, r26   \n\t"
+        "adc %B0, r26   \n\t"
+        "clr r1         \n\t"
+        : "=&r"(intRes)
+        : "d"(charIn1), "d"(intIn2)
+        : "r26");
+#endif
+    return intRes;
+}
+
+/// (longIn1 * longIn2) >> 24
+static inline uint16_t mulU24X24toH16(const uint32_t &longIn1, const uint32_t &longIn2) {
+    uint16_t intRes;
+#if !defined(__AVR__) || defined(NO_ASM)
+    intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
+#else
+    asm volatile(
+        "clr r26         \n\t"
+        "mul %A1, %B2    \n\t"
+        "mov r27, r1     \n\t"
+        "mul %B1, %C2    \n\t"
+        "movw %A0, r0    \n\t"
+        "mul %C1, %C2    \n\t"
+        "add %B0, r0     \n\t"
+        "mul %C1, %B2    \n\t"
+        "add %A0, r0     \n\t"
+        "adc %B0, r1     \n\t"
+        "mul %A1, %C2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %B1, %B2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %C1, %A2    \n\t"
+        "add r27, r0     \n\t"
+        "adc %A0, r1     \n\t"
+        "adc %B0, r26    \n\t"
+        "mul %B1, %A2    \n\t"
+        "add r27, r1     \n\t"
+        "adc %A0, r26    \n\t"
+        "adc %B0, r26    \n\t"
+        "lsr r27         \n\t"
+        "adc %A0, r26    \n\t"
+        "adc %B0, r26    \n\t"
+        "clr r1          \n\t"
+        : "=&r"(intRes)
+        : "d"(longIn1), "d"(longIn2)
+        : "r26", "r27");
+#endif
+    return intRes;
+}
+
+} // namespace math
+} // namespace modules
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@ -1,71 +1,106 @@
 #include "motion.h"
-#include "../hal/shr16.h"

 namespace modules {
 namespace motion {

 Motion motion;

-void Motion::InitAxis(Axis axis) {}
+void Motion::InitAxis(Axis axis) {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i) {
+        // disable the axis and re-init the driver: this will clear the internal
+        // StallGuard data as a result without special handling
+        Disable(axis);
+        axisData[axis].drv.Init(axisParams[axis].params);
+    }
+}

-void Motion::DisableAxis(Axis axis) {}
+void Motion::SetEnabled(Axis axis, bool enabled) {
+    axisData[axis].drv.SetEnabled(axisParams[axis].params, enabled);
+    axisData[axis].enabled = enabled;

-bool Motion::StallGuard(Axis axis) { return false; }
+    if (!axisData[axis].enabled) {
+        // axis is powered off, clear internal StallGuard counters
+        axisData[axis].stall_trig = false;
+        axisData[axis].stall_cnt = 0;
+    }
+}

-void Motion::ClearStallGuardFlag(Axis axis) {}
+void Motion::SetMode(Axis axis, MotorMode mode) {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        axisData[axis].drv.SetMode(mode);
+}

-void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {}
+bool Motion::StallGuard(Axis axis) {
+    return axisData[axis].stall_trig;
+}

-void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {}
+void Motion::ClearStallGuardFlag(Axis axis) {
+    axisData[axis].stall_trig = false;
+}

-uint16_t Motion::CurrentPos(Axis axis) const { return 0; }
+// TODO: not implemented
+void Motion::Home(Axis axis, bool direction) {
+}

-void Motion::Home(Axis axis, bool direction) {}
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
+    if (axisData[axis].ctrl.PlanMoveTo(pos, feedrate)) {
+        // move was queued, prepare the axis
+        if (!axisData[axis].enabled)
+            SetEnabled(axis, true);
+    }
+}

-void Motion::SetMode(MotorMode mode) {}
+pos_t Motion::Position(Axis axis) const {
+    return axisData[axis].ctrl.Position();
+}

-void Motion::Step() {}
+bool Motion::QueueEmpty() const {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        if (!axisData[i].ctrl.QueueEmpty())
+            return false;
+    return true;
+}

-bool Motion::QueueEmpty() const { return false; }
+void Motion::AbortPlannedMoves() {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        axisData[i].ctrl.AbortPlannedMoves();
+}

-void Motion::AbortPlannedMoves() {}
+st_timer_t Motion::Step() {
+    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);
+
+            // axis has been moved, sample StallGuard
+            if (hal::tmc2130::TMC2130::Stall(axisParams[i].params)) {
+                // TODO: on the MK3 a stall is marked as such as 1/2 of a full step is
+                // lost: this is too simplistic for production
+                ++axisData[i].stall_cnt;
+                axisData[i].stall_trig = true;
+            }
+        }
+    }
+
+    // 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() {}

-//@@TODO check the directions
-void StepDirPins::SetIdlerDirUp() {
-    hal::shr16::shr16.SetTMCDir(Axis::Idler, true);
-}
-
-void StepDirPins::SetIdlerDirDown() {
-    hal::shr16::shr16.SetTMCDir(Axis::Idler, false);
-}
-
-void StepDirPins::SetSelectorDirLeft() {
-    hal::shr16::shr16.SetTMCDir(Axis::Selector, true);
-}
-void StepDirPins::SetSelectorDirRight() {
-    hal::shr16::shr16.SetTMCDir(Axis::Selector, false);
-}
-
-void StepDirPins::SetPulleyDirPull() {
-    hal::shr16::shr16.SetTMCDir(Axis::Pulley, true);
-}
-void StepDirPins::SetPulleyDirPush() {
-    hal::shr16::shr16.SetTMCDir(Axis::Pulley, false);
-}
-
-void StepDirPins::StepIdler(uint8_t on) {
-    // PORTD |= idler_step_pin;
-}
-
-void StepDirPins::StepSelector(uint8_t on) {
-    // PORTD |= selector_step_pin;
-}
-
-void StepDirPins::StepPulley(uint8_t on) {
-    // PORTB |= pulley_step_pin;
-}
-
 } // namespace motion
 } // namespace modules
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@ -1,74 +1,100 @@
 #pragma once
-#include <stdint.h>
+#include "../pins.h"
+#include "pulse_gen.h"
+
+namespace modules {

 /// @@TODO
 /// Logic of motor handling
 /// Ideally enable stepping of motors under ISR (all timers have higher priority than serial)
-
-/// input:
-/// motor, direction, speed (step rate), may be acceleration if necessary (not sure)
-/// enable/disable motor current
-/// stealth/normal
-
-/// Motors:
-/// idler
-/// selector
-/// pulley
-
-/// Operations:
-/// setDir();
-/// rotate(speed)
-/// rotate(speed, angle/steps)
-/// home?
-
-namespace modules {
 namespace motion {

-enum Axis {
+using namespace hal::tmc2130;
+using pulse_gen::pos_t;
+using pulse_gen::st_timer_t;
+using pulse_gen::steps_t;
+
+// Check for configuration invariants
+static_assert(
+    (1. / (F_CPU / config::stepTimerFrequencyDivider) * config::stepTimerQuantum)
+        < (1. / config::maxStepFrequency / 2),
+    "stepTimerQuantum must be smaller than the maximal stepping frequency interval");
+
+/// Main axis enumeration
+enum Axis : uint8_t {
    Pulley,
    Selector,
    Idler,
+    _Axis_Last = Idler
 };

-enum MotorMode {
-    Stealth,
-    Normal
+static constexpr uint8_t NUM_AXIS = _Axis_Last + 1;
+
+struct AxisParams {
+    char name;
+    MotorParams params;
+    MotorCurrents currents;
+    MotorMode mode;
+    steps_t jerk;
+    steps_t accel;
 };

-enum IdlerMode {
-    Engage,
-    Disengage
+/// Return the default motor mode for an Axis
+static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) {
+    return axis.stealth ? MotorMode::Stealth : MotorMode::Normal;
+}
+
+/// Static axis configuration
+static constexpr AxisParams axisParams[NUM_AXIS] = {
+    // Pulley
+    {
+        .name = 'P',
+        .params = { .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .uSteps = config::pulley.uSteps },
+        .currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold },
+        .mode = DefaultMotorMode(config::pulley),
+        .jerk = config::pulley.jerk,
+        .accel = config::pulley.accel,
+    },
+    // Selector
+    {
+        .name = 'S',
+        .params = { .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .uSteps = config::selector.uSteps },
+        .currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold },
+        .mode = DefaultMotorMode(config::selector),
+        .jerk = config::selector.jerk,
+        .accel = config::selector.accel,
+    },
+    // Idler
+    {
+        .name = 'I',
+        .params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps },
+        .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold },
+        .mode = DefaultMotorMode(config::idler),
+        .jerk = config::idler.jerk,
+        .accel = config::idler.accel,
+    },
 };

-/// As step and dir pins are randomly scattered on the board for each of the axes/motors
-/// it is convenient to make a common interface for them
-class StepDirPins {
-public:
-    static void SetIdlerDirUp();
-    static void SetIdlerDirDown();
-
-    static void SetSelectorDirLeft();
-    static void SetSelectorDirRight();
-
-    static void SetPulleyDirPull();
-    static void SetPulleyDirPush();
-
-    static void StepIdler(uint8_t on);
-    static void StepSelector(uint8_t on);
-    static void StepPulley(uint8_t on);
-};
-
-/// @@TODO this is subject of discussion and change in the future
 class Motion {
 public:
    inline constexpr Motion() = default;

-    /// 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)
+    /// 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
-    void DisableAxis(Axis axis);
+    /// Set axis power status. One must manually ensure no moves are currently being
+    /// performed by calling QueueEmpty().
+    void SetEnabled(Axis axis, bool enabled);
+
+    /// Disable axis motor. One must manually ensure no moves are currently being
+    /// performed by calling QueueEmpty().
+    void Disable(Axis axis) { SetEnabled(axis, false); }
+
+    /// Set mode of TMC/motors operation. One must manually ensure no moves are currently
+    /// being performed by calling QueueEmpty().
+    void SetMode(Axis axis, MotorMode mode);

    /// @returns true if a stall guard event occurred recently on the axis
    bool StallGuard(Axis axis);
@ -76,40 +102,104 @@ public:
    /// 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(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed);
-
-    /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate
-    /// @param axis axis affected
-    /// @param delta number of steps in either direction
-    /// @param feedrate maximum feedrate/speed after acceleration
-    void PlanMove(Axis axis, int16_t delta, uint16_t feedrate);
-
-    /// @returns current position of an axis
-    /// @param axis axis affected
-    uint16_t CurrentPos(Axis axis) const;
-
    /// Enqueue performing of homing of an axis
-    /// @@TODO
    void Home(Axis axis, bool direction);

-    /// Set mode of TMC/motors operation
-    /// Common for all axes/motors
-    void SetMode(MotorMode mode);
+    /// Enqueue a single axis move in steps starting and ending at zero speed with maximum
+    /// feedrate. Moves can only be enqueued if the axis is not Full().
+    /// @param axis axis affected
+    /// @param pos target position
+    /// @param feedrate maximum feedrate
+    void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate);

-    /// State machine doing all the planning and stepping preparation based on received commands
-    void Step();
+    /// Enqueue a single axis move in steps starting and ending at zero speed with maximum
+    /// feedrate. Moves can only be enqueued if the axis is not Full().
+    /// @param axis axis affected
+    /// @param delta relative to current position
+    /// @param feedrate maximum feedrate
+    void PlanMove(Axis axis, pos_t delta, steps_t feedrate) {
+        PlanMoveTo(axis, Position(axis) + delta, feedrate);
+    }

-    /// @returns true if all planned moves have been finished
+    /// @returns head position of an axis (last enqueued position)
+    /// @param axis axis affected
+    pos_t Position(Axis axis) const;
+
+    /// Fetch the current position of the axis while stepping. This function is expensive!
+    /// It's necessary only in exceptional cases. For regular usage, Position() should
+    /// probably be used instead.
+    /// @param axis axis affected
+    /// @returns the current position of the axis
+    pos_t CurPosition(Axis axis) const { return axisData[axis].ctrl.CurPosition(); }
+
+    /// Set the position of an axis. Should only be called when the queue is empty.
+    /// @param axis axis affected
+    /// @param x position to set
+    void SetPosition(Axis axis, pos_t x) { axisData[axis].ctrl.SetPosition(x); }
+
+    /// Get current acceleration for the selected axis
+    /// @param axis axis affected
+    /// @returns acceleration
+    steps_t Acceleration(Axis axis) {
+        return axisData[axis].ctrl.Acceleration();
+    }
+
+    /// Set acceleration for the selected axis
+    /// @param axis axis affected
+    /// @param accel acceleration
+    void SetAcceleration(Axis axis, steps_t accel) {
+        axisData[axis].ctrl.SetAcceleration(accel);
+    }
+
+    /// State machine doing all the planning and stepping. Called by the stepping ISR.
+    /// @returns the interval for the next tick
+    st_timer_t Step();
+
+    /// @returns true if all planned moves have been finished for all axes
    bool QueueEmpty() const;

+    /// @returns true if all planned moves have been finished for one axis
+    /// @param axis requested
+    bool QueueEmpty(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(); }
+
    /// stop whatever moves are being done
    void AbortPlannedMoves();

-    /// probably higher-level operations knowing the semantic meaning of axes
-
 private:
+    struct AxisData {
+        TMC2130 drv; ///< Motor driver
+        pulse_gen::PulseGen ctrl; ///< Motor controller
+        bool enabled; ///< Axis enabled
+        st_timer_t residual; ///< Axis timer residual
+        uint8_t stall_cnt; ///< Underlying StallGuard lost ustep count
+        bool stall_trig; ///< StallGuard trigger flag
+    };
+
+    /// Helper to initialize AxisData members
+    static AxisData DataForAxis(Axis axis) {
+        return {
+            .drv = {
+                axisParams[axis].params,
+                axisParams[axis].currents,
+                axisParams[axis].mode,
+            },
+            .ctrl = {
+                axisParams[axis].jerk,
+                axisParams[axis].accel,
+            },
+        };
+    }
+
+    /// Dynamic axis data
+    AxisData axisData[NUM_AXIS] = {
+        DataForAxis(Pulley),
+        DataForAxis(Selector),
+        DataForAxis(Idler),
+    };
 };

 /// ISR stepping routine
--- a/src/modules/pulse_gen.cpp
+++ b/src/modules/pulse_gen.cpp
@ -0,0 +1,149 @@
+#include "pulse_gen.h"
+
+namespace modules {
+namespace pulse_gen {
+
+PulseGen::PulseGen(steps_t max_jerk, steps_t acceleration) {
+    // Axis status
+    position = 0;
+    this->max_jerk = max_jerk;
+    this->acceleration = acceleration;
+
+    // Block buffer
+    current_block = nullptr;
+}
+
+void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed) {
+    // Minimum stepper rate 120Hz, maximum 40kHz. If the stepper rate goes above 10kHz,
+    // the stepper interrupt routine groups the pulses by 2 or 4 pulses per interrupt tick.
+    rate_t initial_rate = entry_speed;
+    rate_t final_rate = exit_speed;
+
+    // Limit minimal step rate (Otherwise the timer will overflow.)
+    if (initial_rate < config::minStepRate)
+        initial_rate = config::minStepRate;
+    if (initial_rate > block->nominal_rate)
+        initial_rate = block->nominal_rate;
+    if (final_rate < config::minStepRate)
+        final_rate = config::minStepRate;
+    if (final_rate > block->nominal_rate)
+        final_rate = block->nominal_rate;
+
+    // Don't allow zero acceleration.
+    rate_t acceleration = block->acceleration;
+    if (acceleration == 0)
+        acceleration = 1;
+
+    // estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
+    // (target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration));
+    rate_t initial_rate_sqr = initial_rate * initial_rate;
+    rate_t nominal_rate_sqr = block->nominal_rate * block->nominal_rate;
+    rate_t final_rate_sqr = final_rate * final_rate;
+    rate_t acceleration_x2 = acceleration << 1;
+    // ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
+    steps_t accelerate_steps = (nominal_rate_sqr - initial_rate_sqr + acceleration_x2 - 1) / acceleration_x2;
+    // floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
+    steps_t decelerate_steps = (nominal_rate_sqr - final_rate_sqr) / acceleration_x2;
+    steps_t accel_decel_steps = accelerate_steps + decelerate_steps;
+    // Size of Plateau of Nominal Rate.
+    steps_t plateau_steps = 0;
+
+    // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
+    // have to use intersection_distance() to calculate when to abort acceleration and start braking
+    // in order to reach the final_rate exactly at the end of this block.
+    if (accel_decel_steps < block->steps) {
+        plateau_steps = block->steps - accel_decel_steps;
+    } else {
+        uint32_t acceleration_x4 = acceleration << 2;
+        // Avoid negative numbers
+        if (final_rate_sqr >= initial_rate_sqr) {
+            // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->steps));
+            // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance)
+            // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration);
+            accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1;
+            if (block->steps & 1)
+                accelerate_steps += acceleration_x2;
+            accelerate_steps /= acceleration_x4;
+            accelerate_steps += (block->steps >> 1);
+            if (accelerate_steps > block->steps)
+                accelerate_steps = block->steps;
+        } else {
+            decelerate_steps = initial_rate_sqr - final_rate_sqr;
+            if (block->steps & 1)
+                decelerate_steps += acceleration_x2;
+            decelerate_steps /= acceleration_x4;
+            decelerate_steps += (block->steps >> 1);
+            if (decelerate_steps > block->steps)
+                decelerate_steps = block->steps;
+            accelerate_steps = block->steps - decelerate_steps;
+        }
+    }
+
+    block->accelerate_until = accelerate_steps;
+    block->decelerate_after = accelerate_steps + plateau_steps;
+    block->initial_rate = initial_rate;
+    block->final_rate = final_rate;
+}
+
+bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) {
+    // Prepare to set up new block
+    if (block_index.full())
+        return false;
+    block_t *block = &block_buffer[block_index.back()];
+
+    // Bail if this is a zero-length block
+    block->steps = abs(target - position);
+    if (block->steps <= config::dropSegments) {
+        // behave as-if the block has been scheduled
+        return true;
+    }
+
+    // Direction and speed for this block
+    block->direction = (target >= position);
+    block->nominal_rate = feed_rate;
+
+    // Acceleration of the segment, in steps/sec^2
+    block->acceleration = acceleration;
+    block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / config::stepTimerFrequencyDivider));
+
+    // Perform the trapezoid calculations
+    CalculateTrapezoid(block, max_jerk, max_jerk);
+
+    // Move forward
+    block_index.push();
+    position = target;
+    return true;
+}
+
+pos_t PulseGen::CurPosition() const {
+    pos_t cur_pos = position;
+    circular_index_t iter = block_index;
+
+    // if we have a live block remove the partial offset
+    if (current_block) {
+        cur_pos -= CurBlockShift();
+        iter.pop();
+    }
+
+    // rollback remaining blocks
+    while (!iter.empty()) {
+        cur_pos -= BlockShift(&block_buffer[iter.front()]);
+        iter.pop();
+    }
+
+    return cur_pos;
+}
+
+void PulseGen::AbortPlannedMoves() {
+    // always update to effective position
+    position = CurPosition();
+
+    // destroy the current block
+    if (current_block) {
+        current_block = nullptr;
+        block_index.pop();
+    }
+}
+
+} // namespace motor
+} // namespace modules
--- a/src/modules/pulse_gen.h
+++ b/src/modules/pulse_gen.h
@ -0,0 +1,195 @@
+#pragma once
+#include "speed_table.h"
+#include "../hal/tmc2130.h"
+#include "../hal/circular_buffer.h"
+#include "../cmath.h"
+
+namespace modules {
+
+/// Acceleration ramp and stepper pulse generator
+namespace pulse_gen {
+
+using config::blockBufferSize;
+using hal::tmc2130::TMC2130;
+using math::mulU24X24toH16;
+using speed_table::calc_timer;
+using speed_table::st_timer_t;
+
+typedef CircularIndex<uint8_t, blockBufferSize> circular_index_t;
+typedef uint32_t steps_t; ///< Absolute step units
+typedef uint32_t rate_t; ///< Type for step rates
+typedef int32_t pos_t; ///< Axis position (signed)
+
+class PulseGen {
+public:
+    PulseGen(steps_t max_jerk, steps_t acceleration);
+
+    /// @returns the acceleration for the axis
+    steps_t Acceleration() const { return acceleration; };
+
+    /// Set acceleration for the axis
+    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().
+    /// @param pos target position
+    /// @param feedrate maximum feedrate
+    /// @returns true if the move has been enqueued
+    bool PlanMoveTo(pos_t pos, steps_t feedrate);
+
+    /// Stop whatever moves are being done
+    void AbortPlannedMoves();
+
+    /// @returns the head position of the axis at the end of all moves
+    pos_t Position() const { return position; }
+
+    /// Fetch the current position of the axis while stepping. This function is expensive!
+    /// It's necessary only in exceptional cases. For regular usage, Position() should
+    /// probably be used instead.
+    /// @returns the current position of the axis
+    pos_t CurPosition() const;
+
+    /// 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; }
+
+    /// @returns true if all planned moves have been finished
+    bool QueueEmpty() const { return block_index.empty(); }
+
+    /// @returns false if new moves can still be planned
+    bool Full() const { return block_index.full(); }
+
+    /// Single-step the axis
+    /// @returns the interval for the next tick
+    inline st_timer_t Step(const hal::tmc2130::MotorParams &motorParams) {
+        if (!current_block) {
+            // fetch next block
+            if (!block_index.empty())
+                current_block = &block_buffer[block_index.front()];
+            if (!current_block)
+                return 0;
+
+            // Set direction early so that the direction-change delay is accounted for
+            TMC2130::SetDir(motorParams, current_block->direction);
+
+            // Initializes the trapezoid generator from the current block.
+            deceleration_time = 0;
+            acc_step_rate = uint16_t(current_block->initial_rate);
+            acceleration_time = calc_timer(acc_step_rate, step_loops);
+            steps_completed = 0;
+
+            // Set the nominal step loops to zero to indicate that the timer value is not
+            // known yet. That means, delay the initialization of nominal step rate and
+            // step loops until the steady state is reached.
+            step_loops_nominal = 0;
+        }
+
+        // Step the motor
+        for (uint8_t i = 0; i < step_loops; ++i) {
+            TMC2130::Step(motorParams);
+            if (++steps_completed >= current_block->steps)
+                break;
+        }
+
+        // Calculate new timer value
+        // 13.38-14.63us for steady state,
+        // 25.12us for acceleration / deceleration.
+        st_timer_t timer;
+        if (steps_completed <= current_block->accelerate_until) {
+            // v = t * a   ->   acc_step_rate = acceleration_time * current_block->acceleration_rate
+            acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate);
+            acc_step_rate += uint16_t(current_block->initial_rate);
+            // upper limit
+            if (acc_step_rate > uint16_t(current_block->nominal_rate))
+                acc_step_rate = current_block->nominal_rate;
+            // step_rate to timer interval
+            timer = calc_timer(acc_step_rate, step_loops);
+            acceleration_time += timer;
+        } else if (steps_completed > current_block->decelerate_after) {
+            st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate);
+
+            if (step_rate > acc_step_rate) { // Check step_rate stays positive
+                step_rate = uint16_t(current_block->final_rate);
+            } else {
+                step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
+
+                // lower limit
+                if (step_rate < current_block->final_rate)
+                    step_rate = uint16_t(current_block->final_rate);
+            }
+
+            // Step_rate to timer interval.
+            timer = calc_timer(step_rate, step_loops);
+            deceleration_time += timer;
+        } else {
+            if (!step_loops_nominal) {
+                // Calculation of the steady state timer rate has been delayed to the 1st tick
+                // of the steady state to lower the initial interrupt blocking.
+                timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
+                step_loops_nominal = step_loops;
+            }
+            timer = timer_nominal;
+        }
+
+        // If current block is finished, reset pointer
+        if (steps_completed >= current_block->steps) {
+            current_block = nullptr;
+            block_index.pop();
+        }
+
+        return timer;
+    }
+
+private:
+    /// Motion parameters for the current planned or executing move
+    struct block_t {
+        steps_t steps; ///< Step events
+        bool direction; ///< The direction for this block
+
+        rate_t acceleration_rate; ///< The acceleration rate
+        steps_t accelerate_until; ///< The index of the step event on which to stop acceleration
+        steps_t decelerate_after; ///< The index of the step event on which to start decelerating
+
+        // Settings for the trapezoid generator (runs inside an interrupt handler)
+        rate_t nominal_rate; ///< The nominal step rate for this block in steps/sec
+        rate_t initial_rate; ///< Rate at start of block
+        rate_t final_rate; ///< Rate at exit
+        rate_t acceleration; ///< acceleration steps/sec^2
+    };
+
+    // Block buffer parameters
+    block_t block_buffer[blockBufferSize];
+    circular_index_t block_index;
+    block_t *current_block;
+
+    // Axis data
+    pos_t position; ///< Current axis position
+    steps_t max_jerk; ///< Axis jerk (could be constant)
+    steps_t acceleration; ///< Current axis acceleration
+
+    // Step parameters
+    rate_t acceleration_time, deceleration_time;
+    st_timer_t acc_step_rate; // decelaration start point
+    uint8_t step_loops; // steps per loop
+    uint8_t step_loops_nominal; // steps per loop at nominal speed
+    st_timer_t timer_nominal; // nominal interval
+    steps_t steps_completed; // steps completed
+
+    /// Calculate the trapezoid parameters for the block
+    void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed);
+
+    /// Return the axis shift introduced by the current (live) block
+    inline pos_t CurBlockShift() const {
+        steps_t steps_missing = (current_block->steps - steps_completed);
+        return current_block->direction ? steps_missing : -steps_missing;
+    }
+
+    /// Return the axis shift introduced by the specified full block
+    static inline pos_t BlockShift(const block_t *block) {
+        return block->direction ? block->steps : -block->steps;
+    }
+};
+
+} // namespace pulse_gen
+} // namespace modules
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@ -21,7 +21,7 @@ bool Selector::MoveToSlot(uint8_t slot) {
        return true;

    mm::motion.InitAxis(mm::Selector);
-    mm::motion.PlanMove(mm::Selector, config::selectorSlotPositions[slot] - mm::motion.CurrentPos(mm::Selector), 1000); // @@TODO
+    mm::motion.PlanMove(mm::Selector, config::selectorSlotPositions[slot] - mm::motion.Position(mm::Selector), 1000); // @@TODO
    state = Moving;
    return true;
 }
@ -44,7 +44,7 @@ bool Selector::Step() {
        return false;
    case Ready:
        currentSlot = plannedSlot;
-        mm::motion.DisableAxis(mm::Selector); // turn off selector motor's power every time
+        mm::motion.Disable(mm::Selector); // turn off selector motor's power every time
        return true;
    case Failed:
    default:
--- a/src/modules/speed_table.cpp
+++ b/src/modules/speed_table.cpp
@ -0,0 +1,601 @@
+#include "speed_table.h"
+
+namespace modules {
+namespace speed_table {
+
+#if F_CPU == 16000000
+
+const st_timer_t speed_table_fast[256][2] PROGMEM = {
+    { 62500, 55556 }, { 6944, 3268 }, { 3676, 1176 }, { 2500, 607 }, { 1893, 369 }, { 1524, 249 }, { 1275, 179 }, { 1096, 135 },
+    { 961, 105 }, { 856, 85 }, { 771, 69 }, { 702, 58 }, { 644, 49 }, { 595, 42 }, { 553, 37 }, { 516, 32 },
+    { 484, 28 }, { 456, 25 }, { 431, 23 }, { 408, 20 }, { 388, 19 }, { 369, 16 }, { 353, 16 }, { 337, 14 },
+    { 323, 13 }, { 310, 11 }, { 299, 11 }, { 288, 11 }, { 277, 9 }, { 268, 9 }, { 259, 8 }, { 251, 8 },
+    { 243, 8 }, { 235, 7 }, { 228, 6 }, { 222, 6 }, { 216, 6 }, { 210, 6 }, { 204, 5 }, { 199, 5 },
+    { 194, 5 }, { 189, 4 }, { 185, 4 }, { 181, 4 }, { 177, 4 }, { 173, 4 }, { 169, 4 }, { 165, 3 },
+    { 162, 3 }, { 159, 4 }, { 155, 3 }, { 152, 3 }, { 149, 2 }, { 147, 3 }, { 144, 3 }, { 141, 2 },
+    { 139, 3 }, { 136, 2 }, { 134, 2 }, { 132, 3 }, { 129, 2 }, { 127, 2 }, { 125, 2 }, { 123, 2 },
+    { 121, 2 }, { 119, 1 }, { 118, 2 }, { 116, 2 }, { 114, 1 }, { 113, 2 }, { 111, 2 }, { 109, 1 },
+    { 108, 2 }, { 106, 1 }, { 105, 2 }, { 103, 1 }, { 102, 1 }, { 101, 1 }, { 100, 2 }, { 98, 1 },
+    { 97, 1 }, { 96, 1 }, { 95, 2 }, { 93, 1 }, { 92, 1 }, { 91, 1 }, { 90, 1 }, { 89, 1 },
+    { 88, 1 }, { 87, 1 }, { 86, 1 }, { 85, 1 }, { 84, 1 }, { 83, 0 }, { 83, 1 }, { 82, 1 },
+    { 81, 1 }, { 80, 1 }, { 79, 1 }, { 78, 0 }, { 78, 1 }, { 77, 1 }, { 76, 1 }, { 75, 0 },
+    { 75, 1 }, { 74, 1 }, { 73, 1 }, { 72, 0 }, { 72, 1 }, { 71, 1 }, { 70, 0 }, { 70, 1 },
+    { 69, 0 }, { 69, 1 }, { 68, 1 }, { 67, 0 }, { 67, 1 }, { 66, 0 }, { 66, 1 }, { 65, 0 },
+    { 65, 1 }, { 64, 1 }, { 63, 0 }, { 63, 1 }, { 62, 0 }, { 62, 1 }, { 61, 0 }, { 61, 1 },
+    { 60, 0 }, { 60, 0 }, { 60, 1 }, { 59, 0 }, { 59, 1 }, { 58, 0 }, { 58, 1 }, { 57, 0 },
+    { 57, 1 }, { 56, 0 }, { 56, 0 }, { 56, 1 }, { 55, 0 }, { 55, 1 }, { 54, 0 }, { 54, 0 },
+    { 54, 1 }, { 53, 0 }, { 53, 0 }, { 53, 1 }, { 52, 0 }, { 52, 0 }, { 52, 1 }, { 51, 0 },
+    { 51, 0 }, { 51, 1 }, { 50, 0 }, { 50, 0 }, { 50, 1 }, { 49, 0 }, { 49, 0 }, { 49, 1 },
+    { 48, 0 }, { 48, 0 }, { 48, 1 }, { 47, 0 }, { 47, 0 }, { 47, 0 }, { 47, 1 }, { 46, 0 },
+    { 46, 0 }, { 46, 1 }, { 45, 0 }, { 45, 0 }, { 45, 0 }, { 45, 1 }, { 44, 0 }, { 44, 0 },
+    { 44, 0 }, { 44, 1 }, { 43, 0 }, { 43, 0 }, { 43, 0 }, { 43, 1 }, { 42, 0 }, { 42, 0 },
+    { 42, 0 }, { 42, 1 }, { 41, 0 }, { 41, 0 }, { 41, 0 }, { 41, 0 }, { 41, 1 }, { 40, 0 },
+    { 40, 0 }, { 40, 0 }, { 40, 0 }, { 40, 1 }, { 39, 0 }, { 39, 0 }, { 39, 0 }, { 39, 0 },
+    { 39, 1 }, { 38, 0 }, { 38, 0 }, { 38, 0 }, { 38, 0 }, { 38, 1 }, { 37, 0 }, { 37, 0 },
+    { 37, 0 }, { 37, 0 }, { 37, 0 }, { 37, 1 }, { 36, 0 }, { 36, 0 }, { 36, 0 }, { 36, 0 },
+    { 36, 1 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 0 }, { 35, 1 },
+    { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 0 }, { 34, 1 }, { 33, 0 }, { 33, 0 },
+    { 33, 0 }, { 33, 0 }, { 33, 0 }, { 33, 0 }, { 33, 1 }, { 32, 0 }, { 32, 0 }, { 32, 0 },
+    { 32, 0 }, { 32, 0 }, { 32, 0 }, { 32, 0 }, { 32, 1 }, { 31, 0 }, { 31, 0 }, { 31, 0 },
+    { 31, 0 }, { 31, 0 }, { 31, 0 }, { 31, 1 }, { 30, 0 }, { 30, 0 }, { 30, 0 }, { 30, 0 }
+};
+
+const st_timer_t speed_table_slow[256][2] PROGMEM = {
+    { 62500, 12500 }, { 50000, 8334 }, { 41666, 5952 }, { 35714, 4464 }, { 31250, 3473 }, { 27777, 2777 }, { 25000, 2273 }, { 22727, 1894 },
+    { 20833, 1603 }, { 19230, 1373 }, { 17857, 1191 }, { 16666, 1041 }, { 15625, 920 }, { 14705, 817 }, { 13888, 731 }, { 13157, 657 },
+    { 12500, 596 }, { 11904, 541 }, { 11363, 494 }, { 10869, 453 }, { 10416, 416 }, { 10000, 385 }, { 9615, 356 }, { 9259, 331 },
+    { 8928, 308 }, { 8620, 287 }, { 8333, 269 }, { 8064, 252 }, { 7812, 237 }, { 7575, 223 }, { 7352, 210 }, { 7142, 198 },
+    { 6944, 188 }, { 6756, 178 }, { 6578, 168 }, { 6410, 160 }, { 6250, 153 }, { 6097, 145 }, { 5952, 139 }, { 5813, 132 },
+    { 5681, 126 }, { 5555, 121 }, { 5434, 115 }, { 5319, 111 }, { 5208, 106 }, { 5102, 102 }, { 5000, 99 }, { 4901, 94 },
+    { 4807, 91 }, { 4716, 87 }, { 4629, 84 }, { 4545, 81 }, { 4464, 79 }, { 4385, 75 }, { 4310, 73 }, { 4237, 71 },
+    { 4166, 68 }, { 4098, 66 }, { 4032, 64 }, { 3968, 62 }, { 3906, 60 }, { 3846, 59 }, { 3787, 56 }, { 3731, 55 },
+    { 3676, 53 }, { 3623, 52 }, { 3571, 50 }, { 3521, 49 }, { 3472, 48 }, { 3424, 46 }, { 3378, 45 }, { 3333, 44 },
+    { 3289, 43 }, { 3246, 41 }, { 3205, 41 }, { 3164, 39 }, { 3125, 39 }, { 3086, 38 }, { 3048, 36 }, { 3012, 36 },
+    { 2976, 35 }, { 2941, 35 }, { 2906, 33 }, { 2873, 33 }, { 2840, 32 }, { 2808, 31 }, { 2777, 30 }, { 2747, 30 },
+    { 2717, 29 }, { 2688, 29 }, { 2659, 28 }, { 2631, 27 }, { 2604, 27 }, { 2577, 26 }, { 2551, 26 }, { 2525, 25 },
+    { 2500, 25 }, { 2475, 25 }, { 2450, 23 }, { 2427, 24 }, { 2403, 23 }, { 2380, 22 }, { 2358, 22 }, { 2336, 22 },
+    { 2314, 21 }, { 2293, 21 }, { 2272, 20 }, { 2252, 20 }, { 2232, 20 }, { 2212, 20 }, { 2192, 19 }, { 2173, 18 },
+    { 2155, 19 }, { 2136, 18 }, { 2118, 18 }, { 2100, 17 }, { 2083, 17 }, { 2066, 17 }, { 2049, 17 }, { 2032, 16 },
+    { 2016, 16 }, { 2000, 16 }, { 1984, 16 }, { 1968, 15 }, { 1953, 16 }, { 1937, 14 }, { 1923, 15 }, { 1908, 15 },
+    { 1893, 14 }, { 1879, 14 }, { 1865, 14 }, { 1851, 13 }, { 1838, 14 }, { 1824, 13 }, { 1811, 13 }, { 1798, 13 },
+    { 1785, 12 }, { 1773, 13 }, { 1760, 12 }, { 1748, 12 }, { 1736, 12 }, { 1724, 12 }, { 1712, 12 }, { 1700, 11 },
+    { 1689, 12 }, { 1677, 11 }, { 1666, 11 }, { 1655, 11 }, { 1644, 11 }, { 1633, 10 }, { 1623, 11 }, { 1612, 10 },
+    { 1602, 10 }, { 1592, 10 }, { 1582, 10 }, { 1572, 10 }, { 1562, 10 }, { 1552, 9 }, { 1543, 10 }, { 1533, 9 },
+    { 1524, 9 }, { 1515, 9 }, { 1506, 9 }, { 1497, 9 }, { 1488, 9 }, { 1479, 9 }, { 1470, 9 }, { 1461, 8 },
+    { 1453, 8 }, { 1445, 9 }, { 1436, 8 }, { 1428, 8 }, { 1420, 8 }, { 1412, 8 }, { 1404, 8 }, { 1396, 8 },
+    { 1388, 7 }, { 1381, 8 }, { 1373, 7 }, { 1366, 8 }, { 1358, 7 }, { 1351, 7 }, { 1344, 8 }, { 1336, 7 },
+    { 1329, 7 }, { 1322, 7 }, { 1315, 7 }, { 1308, 6 }, { 1302, 7 }, { 1295, 7 }, { 1288, 6 }, { 1282, 7 },
+    { 1275, 6 }, { 1269, 7 }, { 1262, 6 }, { 1256, 6 }, { 1250, 7 }, { 1243, 6 }, { 1237, 6 }, { 1231, 6 },
+    { 1225, 6 }, { 1219, 6 }, { 1213, 6 }, { 1207, 6 }, { 1201, 5 }, { 1196, 6 }, { 1190, 6 }, { 1184, 5 },
+    { 1179, 6 }, { 1173, 5 }, { 1168, 6 }, { 1162, 5 }, { 1157, 5 }, { 1152, 6 }, { 1146, 5 }, { 1141, 5 },
+    { 1136, 5 }, { 1131, 5 }, { 1126, 5 }, { 1121, 5 }, { 1116, 5 }, { 1111, 5 }, { 1106, 5 }, { 1101, 5 },
+    { 1096, 5 }, { 1091, 5 }, { 1086, 4 }, { 1082, 5 }, { 1077, 5 }, { 1072, 4 }, { 1068, 5 }, { 1063, 4 },
+    { 1059, 5 }, { 1054, 4 }, { 1050, 4 }, { 1046, 5 }, { 1041, 4 }, { 1037, 4 }, { 1033, 5 }, { 1028, 4 },
+    { 1024, 4 }, { 1020, 4 }, { 1016, 4 }, { 1012, 4 }, { 1008, 4 }, { 1004, 4 }, { 1000, 4 }, { 996, 4 },
+    { 992, 4 }, { 988, 4 }, { 984, 4 }, { 980, 4 }, { 976, 4 }, { 972, 4 }, { 968, 3 }, { 965, 3 }
+};
+
+#elif F_CPU == 20000000
+
+const st_timer_t speed_table_fast[256][2] PROGMEM = {
+    { 62500, 54055 },
+    { 8445, 3917 },
+    { 4528, 1434 },
+    { 3094, 745 },
+    { 2349, 456 },
+    { 1893, 307 },
+    { 1586, 222 },
+    { 1364, 167 },
+    { 1197, 131 },
+    { 1066, 105 },
+    { 961, 86 },
+    { 875, 72 },
+    { 803, 61 },
+    { 742, 53 },
+    { 689, 45 },
+    { 644, 40 },
+    { 604, 35 },
+    { 569, 32 },
+    { 537, 28 },
+    { 509, 25 },
+    { 484, 23 },
+    { 461, 21 },
+    { 440, 19 },
+    { 421, 17 },
+    { 404, 16 },
+    { 388, 15 },
+    { 373, 14 },
+    { 359, 13 },
+    { 346, 12 },
+    { 334, 11 },
+    { 323, 10 },
+    { 313, 10 },
+    { 303, 9 },
+    { 294, 9 },
+    { 285, 8 },
+    { 277, 7 },
+    { 270, 8 },
+    { 262, 7 },
+    { 255, 6 },
+    { 249, 6 },
+    { 243, 6 },
+    { 237, 6 },
+    { 231, 5 },
+    { 226, 5 },
+    { 221, 5 },
+    { 216, 5 },
+    { 211, 4 },
+    { 207, 5 },
+    { 202, 4 },
+    { 198, 4 },
+    { 194, 4 },
+    { 190, 3 },
+    { 187, 4 },
+    { 183, 3 },
+    { 180, 3 },
+    { 177, 4 },
+    { 173, 3 },
+    { 170, 3 },
+    { 167, 2 },
+    { 165, 3 },
+    { 162, 3 },
+    { 159, 2 },
+    { 157, 3 },
+    { 154, 2 },
+    { 152, 3 },
+    { 149, 2 },
+    { 147, 2 },
+    { 145, 2 },
+    { 143, 2 },
+    { 141, 2 },
+    { 139, 2 },
+    { 137, 2 },
+    { 135, 2 },
+    { 133, 2 },
+    { 131, 2 },
+    { 129, 1 },
+    { 128, 2 },
+    { 126, 2 },
+    { 124, 1 },
+    { 123, 2 },
+    { 121, 1 },
+    { 120, 2 },
+    { 118, 1 },
+    { 117, 1 },
+    { 116, 2 },
+    { 114, 1 },
+    { 113, 1 },
+    { 112, 2 },
+    { 110, 1 },
+    { 109, 1 },
+    { 108, 1 },
+    { 107, 2 },
+    { 105, 1 },
+    { 104, 1 },
+    { 103, 1 },
+    { 102, 1 },
+    { 101, 1 },
+    { 100, 1 },
+    { 99, 1 },
+    { 98, 1 },
+    { 97, 1 },
+    { 96, 1 },
+    { 95, 1 },
+    { 94, 1 },
+    { 93, 1 },
+    { 92, 1 },
+    { 91, 0 },
+    { 91, 1 },
+    { 90, 1 },
+    { 89, 1 },
+    { 88, 1 },
+    { 87, 0 },
+    { 87, 1 },
+    { 86, 1 },
+    { 85, 1 },
+    { 84, 0 },
+    { 84, 1 },
+    { 83, 1 },
+    { 82, 1 },
+    { 81, 0 },
+    { 81, 1 },
+    { 80, 1 },
+    { 79, 0 },
+    { 79, 1 },
+    { 78, 0 },
+    { 78, 1 },
+    { 77, 1 },
+    { 76, 0 },
+    { 76, 1 },
+    { 75, 0 },
+    { 75, 1 },
+    { 74, 1 },
+    { 73, 0 },
+    { 73, 1 },
+    { 72, 0 },
+    { 72, 1 },
+    { 71, 0 },
+    { 71, 1 },
+    { 70, 0 },
+    { 70, 1 },
+    { 69, 0 },
+    { 69, 1 },
+    { 68, 0 },
+    { 68, 1 },
+    { 67, 0 },
+    { 67, 1 },
+    { 66, 0 },
+    { 66, 1 },
+    { 65, 0 },
+    { 65, 0 },
+    { 65, 1 },
+    { 64, 0 },
+    { 64, 1 },
+    { 63, 0 },
+    { 63, 1 },
+    { 62, 0 },
+    { 62, 0 },
+    { 62, 1 },
+    { 61, 0 },
+    { 61, 1 },
+    { 60, 0 },
+    { 60, 0 },
+    { 60, 1 },
+    { 59, 0 },
+    { 59, 0 },
+    { 59, 1 },
+    { 58, 0 },
+    { 58, 0 },
+    { 58, 1 },
+    { 57, 0 },
+    { 57, 0 },
+    { 57, 1 },
+    { 56, 0 },
+    { 56, 0 },
+    { 56, 1 },
+    { 55, 0 },
+    { 55, 0 },
+    { 55, 1 },
+    { 54, 0 },
+    { 54, 0 },
+    { 54, 1 },
+    { 53, 0 },
+    { 53, 0 },
+    { 53, 0 },
+    { 53, 1 },
+    { 52, 0 },
+    { 52, 0 },
+    { 52, 1 },
+    { 51, 0 },
+    { 51, 0 },
+    { 51, 0 },
+    { 51, 1 },
+    { 50, 0 },
+    { 50, 0 },
+    { 50, 0 },
+    { 50, 1 },
+    { 49, 0 },
+    { 49, 0 },
+    { 49, 0 },
+    { 49, 1 },
+    { 48, 0 },
+    { 48, 0 },
+    { 48, 0 },
+    { 48, 1 },
+    { 47, 0 },
+    { 47, 0 },
+    { 47, 0 },
+    { 47, 1 },
+    { 46, 0 },
+    { 46, 0 },
+    { 46, 0 },
+    { 46, 0 },
+    { 46, 1 },
+    { 45, 0 },
+    { 45, 0 },
+    { 45, 0 },
+    { 45, 1 },
+    { 44, 0 },
+    { 44, 0 },
+    { 44, 0 },
+    { 44, 0 },
+    { 44, 1 },
+    { 43, 0 },
+    { 43, 0 },
+    { 43, 0 },
+    { 43, 0 },
+    { 43, 1 },
+    { 42, 0 },
+    { 42, 0 },
+    { 42, 0 },
+    { 42, 0 },
+    { 42, 0 },
+    { 42, 1 },
+    { 41, 0 },
+    { 41, 0 },
+    { 41, 0 },
+    { 41, 0 },
+    { 41, 0 },
+    { 41, 1 },
+    { 40, 0 },
+    { 40, 0 },
+    { 40, 0 },
+    { 40, 0 },
+    { 40, 1 },
+    { 39, 0 },
+    { 39, 0 },
+    { 39, 0 },
+    { 39, 0 },
+    { 39, 0 },
+    { 39, 0 },
+    { 39, 1 },
+    { 38, 0 },
+    { 38, 0 },
+    { 38, 0 },
+    { 38, 0 },
+    { 38, 0 },
+};
+
+const st_timer_t speed_table_slow[256][2] PROGMEM = {
+    { 62500, 10417 },
+    { 52083, 7441 },
+    { 44642, 5580 },
+    { 39062, 4340 },
+    { 34722, 3472 },
+    { 31250, 2841 },
+    { 28409, 2368 },
+    { 26041, 2003 },
+    { 24038, 1717 },
+    { 22321, 1488 },
+    { 20833, 1302 },
+    { 19531, 1149 },
+    { 18382, 1021 },
+    { 17361, 914 },
+    { 16447, 822 },
+    { 15625, 745 },
+    { 14880, 676 },
+    { 14204, 618 },
+    { 13586, 566 },
+    { 13020, 520 },
+    { 12500, 481 },
+    { 12019, 445 },
+    { 11574, 414 },
+    { 11160, 385 },
+    { 10775, 359 },
+    { 10416, 336 },
+    { 10080, 315 },
+    { 9765, 296 },
+    { 9469, 278 },
+    { 9191, 263 },
+    { 8928, 248 },
+    { 8680, 235 },
+    { 8445, 222 },
+    { 8223, 211 },
+    { 8012, 200 },
+    { 7812, 191 },
+    { 7621, 181 },
+    { 7440, 173 },
+    { 7267, 165 },
+    { 7102, 158 },
+    { 6944, 151 },
+    { 6793, 145 },
+    { 6648, 138 },
+    { 6510, 133 },
+    { 6377, 127 },
+    { 6250, 123 },
+    { 6127, 118 },
+    { 6009, 113 },
+    { 5896, 109 },
+    { 5787, 106 },
+    { 5681, 101 },
+    { 5580, 98 },
+    { 5482, 95 },
+    { 5387, 91 },
+    { 5296, 88 },
+    { 5208, 86 },
+    { 5122, 82 },
+    { 5040, 80 },
+    { 4960, 78 },
+    { 4882, 75 },
+    { 4807, 73 },
+    { 4734, 70 },
+    { 4664, 69 },
+    { 4595, 67 },
+    { 4528, 64 },
+    { 4464, 63 },
+    { 4401, 61 },
+    { 4340, 60 },
+    { 4280, 58 },
+    { 4222, 56 },
+    { 4166, 55 },
+    { 4111, 53 },
+    { 4058, 52 },
+    { 4006, 51 },
+    { 3955, 49 },
+    { 3906, 48 },
+    { 3858, 48 },
+    { 3810, 45 },
+    { 3765, 45 },
+    { 3720, 44 },
+    { 3676, 43 },
+    { 3633, 42 },
+    { 3591, 40 },
+    { 3551, 40 },
+    { 3511, 39 },
+    { 3472, 38 },
+    { 3434, 38 },
+    { 3396, 36 },
+    { 3360, 36 },
+    { 3324, 35 },
+    { 3289, 34 },
+    { 3255, 34 },
+    { 3221, 33 },
+    { 3188, 32 },
+    { 3156, 31 },
+    { 3125, 31 },
+    { 3094, 31 },
+    { 3063, 30 },
+    { 3033, 29 },
+    { 3004, 28 },
+    { 2976, 28 },
+    { 2948, 28 },
+    { 2920, 27 },
+    { 2893, 27 },
+    { 2866, 26 },
+    { 2840, 25 },
+    { 2815, 25 },
+    { 2790, 25 },
+    { 2765, 24 },
+    { 2741, 24 },
+    { 2717, 24 },
+    { 2693, 23 },
+    { 2670, 22 },
+    { 2648, 22 },
+    { 2626, 22 },
+    { 2604, 22 },
+    { 2582, 21 },
+    { 2561, 21 },
+    { 2540, 20 },
+    { 2520, 20 },
+    { 2500, 20 },
+    { 2480, 20 },
+    { 2460, 19 },
+    { 2441, 19 },
+    { 2422, 19 },
+    { 2403, 18 },
+    { 2385, 18 },
+    { 2367, 18 },
+    { 2349, 17 },
+    { 2332, 18 },
+    { 2314, 17 },
+    { 2297, 16 },
+    { 2281, 17 },
+    { 2264, 16 },
+    { 2248, 16 },
+    { 2232, 16 },
+    { 2216, 16 },
+    { 2200, 15 },
+    { 2185, 15 },
+    { 2170, 15 },
+    { 2155, 15 },
+    { 2140, 15 },
+    { 2125, 14 },
+    { 2111, 14 },
+    { 2097, 14 },
+    { 2083, 14 },
+    { 2069, 14 },
+    { 2055, 13 },
+    { 2042, 13 },
+    { 2029, 13 },
+    { 2016, 13 },
+    { 2003, 13 },
+    { 1990, 13 },
+    { 1977, 12 },
+    { 1965, 12 },
+    { 1953, 13 },
+    { 1940, 11 },
+    { 1929, 12 },
+    { 1917, 12 },
+    { 1905, 12 },
+    { 1893, 11 },
+    { 1882, 11 },
+    { 1871, 11 },
+    { 1860, 11 },
+    { 1849, 11 },
+    { 1838, 11 },
+    { 1827, 11 },
+    { 1816, 10 },
+    { 1806, 11 },
+    { 1795, 10 },
+    { 1785, 10 },
+    { 1775, 10 },
+    { 1765, 10 },
+    { 1755, 10 },
+    { 1745, 9 },
+    { 1736, 10 },
+    { 1726, 9 },
+    { 1717, 10 },
+    { 1707, 9 },
+    { 1698, 9 },
+    { 1689, 9 },
+    { 1680, 9 },
+    { 1671, 9 },
+    { 1662, 9 },
+    { 1653, 9 },
+    { 1644, 8 },
+    { 1636, 9 },
+    { 1627, 8 },
+    { 1619, 9 },
+    { 1610, 8 },
+    { 1602, 8 },
+    { 1594, 8 },
+    { 1586, 8 },
+    { 1578, 8 },
+    { 1570, 8 },
+    { 1562, 8 },
+    { 1554, 7 },
+    { 1547, 8 },
+    { 1539, 8 },
+    { 1531, 7 },
+    { 1524, 8 },
+    { 1516, 7 },
+    { 1509, 7 },
+    { 1502, 7 },
+    { 1495, 7 },
+    { 1488, 7 },
+    { 1481, 7 },
+    { 1474, 7 },
+    { 1467, 7 },
+    { 1460, 7 },
+    { 1453, 7 },
+    { 1446, 6 },
+    { 1440, 7 },
+    { 1433, 7 },
+    { 1426, 6 },
+    { 1420, 6 },
+    { 1414, 7 },
+    { 1407, 6 },
+    { 1401, 6 },
+    { 1395, 7 },
+    { 1388, 6 },
+    { 1382, 6 },
+    { 1376, 6 },
+    { 1370, 6 },
+    { 1364, 6 },
+    { 1358, 6 },
+    { 1352, 6 },
+    { 1346, 5 },
+    { 1341, 6 },
+    { 1335, 6 },
+    { 1329, 5 },
+    { 1324, 6 },
+    { 1318, 5 },
+    { 1313, 6 },
+    { 1307, 5 },
+    { 1302, 6 },
+    { 1296, 5 },
+    { 1291, 5 },
+    { 1286, 6 },
+    { 1280, 5 },
+    { 1275, 5 },
+    { 1270, 5 },
+    { 1265, 5 },
+    { 1260, 5 },
+    { 1255, 5 },
+    { 1250, 5 },
+    { 1245, 5 },
+    { 1240, 5 },
+    { 1235, 5 },
+    { 1230, 5 },
+    { 1225, 5 },
+    { 1220, 5 },
+    { 1215, 4 },
+    { 1211, 5 },
+    { 1206, 5 },
+    { 1201, 5 },
+};
+
+#endif
+
+} // namespace speed_table
+} // namespace modules
--- a/src/modules/speed_table.h
+++ b/src/modules/speed_table.h
@ -0,0 +1,66 @@
+#pragma once
+#include "../config/config.h"
+#include "../hal/progmem.h"
+#include "../hal/cpu.h"
+#include "math.h"
+
+namespace modules {
+
+/// Speed  tables for acceleration calculations
+namespace speed_table {
+
+typedef uint16_t st_timer_t;
+
+/// CPU timer frequency divider required for the speed tables
+static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000,
+    "speed tables not compatible for the requested frequency");
+
+/// Lookup table for rates equal or higher than 8*256
+extern const st_timer_t speed_table_fast[256][2] PROGMEM;
+
+/// Lookup table for lower step rates
+extern const st_timer_t speed_table_slow[256][2] PROGMEM;
+
+/// Calculate the next timer interval and steps according to current step rate
+static inline st_timer_t calc_timer(st_timer_t step_rate, uint8_t &step_loops) {
+    if (step_rate > config::maxStepFrequency)
+        step_rate = config::maxStepFrequency;
+    if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
+        step_rate = (step_rate >> 2) & 0x3fff;
+        step_loops = 4;
+    } else if (step_rate > 10000) { // If steprate > 10kHz >> step 2 times
+        step_rate = (step_rate >> 1) & 0x7fff;
+        step_loops = 2;
+    } else {
+        step_loops = 1;
+    }
+
+    using modules::math::mulU8X16toH16;
+    namespace pm = hal::progmem;
+
+    st_timer_t timer; // calculated interval
+
+    if (step_rate < (F_CPU / 500000))
+        step_rate = (F_CPU / 500000);
+    step_rate -= (F_CPU / 500000); // Correct for minimal speed
+    if (step_rate >= (8 * 256)) { // higher step rate
+        const uint16_t *table_address = &speed_table_fast[(uint8_t)(step_rate >> 8)][0];
+        uint8_t tmp_step_rate = (step_rate & 0x00ff);
+        uint16_t gain = pm::read_word(table_address + 1);
+        timer = mulU8X16toH16(tmp_step_rate, gain);
+        timer = pm::read_word(table_address) - timer;
+    } else { // lower step rates
+        const uint16_t *table_address = &speed_table_slow[0][0];
+        table_address += (step_rate >> 2) & 0xfffe;
+        timer = pm::read_word(table_address);
+        timer -= ((pm::read_word(table_address + 1) * (uint8_t)(step_rate & 0x0007)) >> 3);
+    }
+    if (timer < 100) {
+        // 20kHz this should never happen
+        timer = 100;
+    }
+    return timer;
+}
+
+} // namespace speed_table
+} // namespace modules
--- a/src/pins.h
+++ b/src/pins.h
@ -13,3 +13,15 @@ static constexpr hal::gpio::GPIO_pin SHR16_CLOCK = { GPIOC, 7 }; ///SHCP

 static constexpr hal::gpio::GPIO_pin USART_RX = { GPIOD, 2 };
 static constexpr hal::gpio::GPIO_pin USART_TX = { GPIOD, 3 };
+
+static constexpr hal::gpio::GPIO_pin IDLER_CS_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin IDLER_SG_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin IDLER_STEP_PIN = { GPIOB, 0 }; // TODO
+
+static constexpr hal::gpio::GPIO_pin PULLEY_CS_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin PULLEY_SG_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin PULLEY_STEP_PIN = { GPIOB, 0 }; // TODO
+
+static constexpr hal::gpio::GPIO_pin SELECTOR_CS_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin SELECTOR_SG_PIN = { GPIOB, 0 }; // TODO
+static constexpr hal::gpio::GPIO_pin SELECTOR_STEP_PIN = { GPIOB, 0 }; // TODO
--- a/tests/unit/logic/cut_filament/CMakeLists.txt
+++ b/tests/unit/logic/cut_filament/CMakeLists.txt
@ -15,11 +15,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_cut_filament.cpp
--- a/tests/unit/logic/eject_filament/CMakeLists.txt
+++ b/tests/unit/logic/eject_filament/CMakeLists.txt
@ -15,11 +15,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_eject_filament.cpp
--- a/tests/unit/logic/feed_to_bondtech/CMakeLists.txt
+++ b/tests/unit/logic/feed_to_bondtech/CMakeLists.txt
@ -12,11 +12,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_feed_to_bondtech.cpp
--- a/tests/unit/logic/feed_to_finda/CMakeLists.txt
+++ b/tests/unit/logic/feed_to_finda/CMakeLists.txt
@ -12,11 +12,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_feed_to_finda.cpp
--- a/tests/unit/logic/load_filament/CMakeLists.txt
+++ b/tests/unit/logic/load_filament/CMakeLists.txt
@ -14,11 +14,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_load_filament.cpp
--- a/tests/unit/logic/stubs/stub_motion.cpp
+++ b/tests/unit/logic/stubs/stub_motion.cpp
@ -18,8 +18,8 @@ void Motion::InitAxis(Axis axis) {
    axes[axis].enabled = true;
 }

-void Motion::DisableAxis(Axis axis) {
-    axes[axis].enabled = false;
+void Motion::SetEnabled(Axis axis, bool enabled) {
+    axes[axis].enabled = enabled;
 }

 bool Motion::StallGuard(Axis axis) {
@ -30,18 +30,11 @@ void Motion::ClearStallGuardFlag(Axis axis) {
    axes[axis].stallGuard = false;
 }

-void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {
-    axes[Pulley].targetPos = axes[Pulley].pos + pulley;
-    axes[Idler].targetPos = axes[Idler].pos + idler;
-    axes[Selector].targetPos = axes[Selector].pos + selector;
-    // speeds and feedrates are not simulated yet
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
+    axes[axis].targetPos = pos;
 }

-void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {
-    axes[axis].targetPos = axes[axis].pos + delta;
-}
-
-uint16_t Motion::CurrentPos(Axis axis) const {
+pos_t Motion::Position(Axis axis) const {
    return axes[axis].pos;
 }

@ -49,16 +42,17 @@ void Motion::Home(Axis axis, bool direction) {
    axes[Pulley].homed = true;
 }

-void Motion::SetMode(MotorMode mode) {
+void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) {
 }

-void Motion::Step() {
+st_timer_t Motion::Step() {
    for (uint8_t i = 0; i < 3; ++i) {
        if (axes[i].pos != axes[i].targetPos) {
            int8_t dirInc = (axes[i].pos < axes[i].targetPos) ? 1 : -1;
            axes[i].pos += dirInc;
        }
    }
+    return 0;
 }

 bool Motion::QueueEmpty() const {
--- a/tests/unit/logic/stubs/stub_motion.h
+++ b/tests/unit/logic/stubs/stub_motion.h
@ -5,8 +5,8 @@ namespace modules {
 namespace motion {

 struct AxisSim {
-    int32_t pos;
-    int32_t targetPos;
+    pos_t pos;
+    pos_t targetPos;
    bool enabled;
    bool homed;
    bool stallGuard;
--- a/tests/unit/logic/tool_change/CMakeLists.txt
+++ b/tests/unit/logic/tool_change/CMakeLists.txt
@ -17,11 +17,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_tool_change.cpp
--- a/tests/unit/logic/unload_filament/CMakeLists.txt
+++ b/tests/unit/logic/unload_filament/CMakeLists.txt
@ -14,11 +14,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_unload_filament.cpp
--- a/tests/unit/logic/unload_to_finda/CMakeLists.txt
+++ b/tests/unit/logic/unload_to_finda/CMakeLists.txt
@ -12,11 +12,13 @@ add_executable(
  ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp
  ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
  ${MODULES_STUBS_DIR}/stub_adc.cpp
  ${MODULES_STUBS_DIR}/stub_eeprom.cpp
  ${MODULES_STUBS_DIR}/stub_gpio.cpp
  ${MODULES_STUBS_DIR}/stub_shr16.cpp
  ${MODULES_STUBS_DIR}/stub_timebase.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
  ${LOGIC_STUBS_DIR}/main_loop_stub.cpp
  ${LOGIC_STUBS_DIR}/stub_motion.cpp
  test_unload_to_finda.cpp
--- a/tests/unit/modules/CMakeLists.txt
+++ b/tests/unit/modules/CMakeLists.txt
@ -1,3 +1,6 @@
 add_subdirectory(buttons)
 add_subdirectory(leds)
 add_subdirectory(protocol)
+add_subdirectory(speed_table)
+add_subdirectory(pulse_gen)
+add_subdirectory(motion)
--- a/tests/unit/modules/motion/CMakeLists.txt
+++ b/tests/unit/modules/motion/CMakeLists.txt
@ -0,0 +1,19 @@
+# define the test executable
+add_executable(
+  motion_tests
+  ${CMAKE_SOURCE_DIR}/src/modules/motion.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp
+  ${MODULES_STUBS_DIR}/stub_gpio.cpp
+  ${MODULES_STUBS_DIR}/stub_shr16.cpp
+  ${MODULES_STUBS_DIR}/stub_tmc2130.cpp
+  test_motion.cpp
+  )
+
+# define required search paths
+target_include_directories(
+  motion_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal
+  )
+
+# tell build system about the test case
+add_catch_test(motion_tests)
--- a/tests/unit/modules/motion/test_motion.cpp
+++ b/tests/unit/modules/motion/test_motion.cpp
@ -0,0 +1,169 @@
+#include "catch2/catch.hpp"
+#include "motion.h"
+
+using namespace modules::motion;
+
+// Perform Step() until all moves are completed, returning the number of steps performed.
+// Ensure the move doesn't run forever, making the test fail reliably.
+ssize_t stepUntilDone(size_t maxSteps = 100000) {
+    for (size_t i = 0; i != maxSteps; ++i)
+        if (!motion.Step())
+            return i;
+
+    // number of steps exceeded
+    return -1;
+}
+
+TEST_CASE("motion::basic", "[motion]") {
+    // initial state
+    REQUIRE(motion.QueueEmpty());
+    REQUIRE(motion.Position(Idler) == 0);
+
+    // enqueue a single move
+    motion.PlanMoveTo(Idler, 10, 1);
+    REQUIRE(!motion.QueueEmpty());
+
+    // perform the move
+    REQUIRE(stepUntilDone() == 10);
+    REQUIRE(motion.QueueEmpty());
+
+    // check positions
+    REQUIRE(motion.Position(Idler) == 10);
+}
+
+TEST_CASE("motion::dual_move_fwd", "[motion]") {
+    // check for configuration values that we cannot change but should match for this test
+    // to function as expected (maybe this should be a static_assert?)
+    REQUIRE(config::idler.jerk == config::selector.jerk);
+
+    // enqueue moves on two axes
+    REQUIRE(motion.QueueEmpty());
+
+    // ensure the same acceleration is set on both
+    motion.SetAcceleration(Idler, motion.Acceleration(Selector));
+    REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector));
+
+    // plan two moves at the same speed and acceleration
+    motion.PlanMoveTo(Idler, 10, 1);
+    motion.PlanMoveTo(Selector, 10, 1);
+
+    // perform the move, which should be perfectly merged
+    REQUIRE(stepUntilDone() == 10);
+    REQUIRE(motion.QueueEmpty());
+
+    // check for final axis positions
+    REQUIRE(motion.Position(Idler) == 10);
+    REQUIRE(motion.Position(Selector) == 10);
+}
+
+TEST_CASE("motion::dual_move_inv", "[motion]") {
+    // check for configuration values that we cannot change but should match for this test
+    // to function as expected (maybe this should be a static_assert?)
+    REQUIRE(config::idler.jerk == config::selector.jerk);
+
+    // enqueue moves on two axes
+    REQUIRE(motion.QueueEmpty());
+
+    // ensure the same acceleration is set on both
+    motion.SetAcceleration(Idler, motion.Acceleration(Selector));
+    REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector));
+
+    // set two different starting points
+    motion.SetPosition(Idler, 0);
+    motion.SetPosition(Selector, 5);
+
+    // plan two moves at the same speed and acceleration: like in the previous
+    // test this should *also* reduce to the same steps being performed
+    motion.PlanMove(Idler, 10, 1);
+    motion.PlanMove(Selector, -10, 1);
+
+    // perform the move, which should be perfectly merged
+    REQUIRE(stepUntilDone() == 10);
+    REQUIRE(motion.QueueEmpty());
+
+    // check for final axis positions
+    REQUIRE(motion.Position(Idler) == 10);
+    REQUIRE(motion.Position(Selector) == -5);
+}
+
+TEST_CASE("motion::dual_move_complex", "[motion]") {
+    // enqueue two completely different moves on two axes
+    REQUIRE(motion.QueueEmpty());
+
+    // set custom acceleration values
+    motion.SetAcceleration(Idler, 10);
+    motion.SetAcceleration(Selector, 20);
+
+    // plan two moves with difference accelerations
+    motion.PlanMoveTo(Idler, 10, 1);
+    motion.PlanMoveTo(Selector, 10, 1);
+
+    // perform the move, which should take less iterations than the sum of both
+    REQUIRE(stepUntilDone(20));
+    REQUIRE(motion.QueueEmpty());
+
+    // check for final axis positions
+    REQUIRE(motion.Position(Idler) == 10);
+    REQUIRE(motion.Position(Selector) == 10);
+}
+
+TEST_CASE("motion::triple_move", "[motion]") {
+    // check that we can move three axes at the same time
+    motion.PlanMoveTo(Idler, 10, 1);
+    motion.PlanMoveTo(Selector, 20, 1);
+    motion.PlanMoveTo(Pulley, 30, 1);
+
+    // perform the move with a maximum step limit
+    REQUIRE(stepUntilDone(10 + 20 + 30));
+
+    // check queue status
+    REQUIRE(motion.QueueEmpty());
+
+    // check for final axis positions
+    REQUIRE(motion.Position(Idler) == 10);
+    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);
+    }
+}
--- a/tests/unit/modules/pulse_gen/CMakeLists.txt
+++ b/tests/unit/modules/pulse_gen/CMakeLists.txt
@ -0,0 +1,14 @@
+# define the test executable
+add_executable(
+  pulse_gen_tests
+  ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp
+  ${MODULES_STUBS_DIR}/stub_gpio.cpp ${MODULES_STUBS_DIR}/stub_shr16.cpp test_pulse_gen.cpp
+  )
+
+# define required search paths
+target_include_directories(
+  pulse_gen_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal
+  )
+
+# tell build system about the test case
+add_catch_test(pulse_gen_tests)
--- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp
+++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp
@ -0,0 +1,287 @@
+#include "catch2/catch.hpp"
+#include "pulse_gen.h"
+#include "../pins.h"
+#include <stdio.h>
+
+using Catch::Matchers::Equals;
+using namespace modules::pulse_gen;
+using hal::gpio::Level;
+using hal::gpio::ReadPin;
+using hal::tmc2130::MotorParams;
+
+namespace hal {
+namespace shr16 {
+extern uint8_t shr16_tmc_dir;
+} // namespace shr16
+} // namespace hal
+
+// Conveniently read the direction set into the lower-level shift register
+bool getTMCDir(const MotorParams &mp) {
+    return (hal::shr16::shr16_tmc_dir & (1 << mp.idx)) ^ mp.dirOn;
+}
+
+// Perform Step() until the move is completed, returning the number of steps performed.
+// Ensure the move doesn't run forever, making the test fail reliably.
+ssize_t stepUntilDone(PulseGen &pg, const MotorParams &mp, size_t maxSteps = 100000) {
+    for (size_t i = 0; i != maxSteps; ++i)
+        if (!pg.Step(mp))
+            return i;
+
+    // number of steps exceeded
+    return -1;
+}
+
+TEST_CASE("pulse_gen::basic", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // perform a simple move
+    REQUIRE(pg.Position() == 0);
+    pg.PlanMoveTo(10, 1);
+    REQUIRE(stepUntilDone(pg, mp) == 10);
+    REQUIRE(pg.Position() == 10);
+
+    // return to zero
+    pg.PlanMoveTo(0, 1);
+    REQUIRE(stepUntilDone(pg, mp) == 10);
+    REQUIRE(pg.Position() == 0);
+
+    // don't move
+    pg.PlanMoveTo(0, 1);
+    REQUIRE(stepUntilDone(pg, mp) == 0);
+    REQUIRE(pg.Position() == 0);
+}
+
+TEST_CASE("pulse_gen::step_dir", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // perform a forward move
+    REQUIRE(pg.Position() == 0);
+    pg.PlanMoveTo(10, 10);
+    REQUIRE(stepUntilDone(pg, mp) == 10);
+    REQUIRE(pg.Position() == 10);
+
+    // check underlying driver direction
+    REQUIRE(getTMCDir(mp));
+
+    // move in reverse
+    pg.PlanMoveTo(0, 10);
+    REQUIRE(stepUntilDone(pg, mp) == 10);
+    REQUIRE(pg.Position() == 0);
+
+    // check underlying driver direction
+    REQUIRE(!getTMCDir(mp));
+
+    // forward again (should match initial state)
+    pg.PlanMoveTo(5, 10);
+    CHECK(stepUntilDone(pg, mp) == 5);
+    REQUIRE(getTMCDir(mp));
+}
+
+TEST_CASE("pulse_gen::step_count", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // step manually, ensuring each step is accounted for
+    REQUIRE(pg.Position() == 0);
+    pg.PlanMoveTo(10, 10);
+    bool st = ReadPin(IDLER_STEP_PIN) == Level::high;
+    for (size_t i = 0; i != 10; ++i) {
+        // check current axis position
+        REQUIRE((pos_t)i == pg.CurPosition());
+
+        // perform the step
+        REQUIRE(pg.Step(mp) > 0);
+        bool newSt = ReadPin(IDLER_STEP_PIN) == Level::high;
+
+        // assuming DEDGE each step should toggle the pin
+        REQUIRE(newSt != st);
+        st = newSt;
+    }
+
+    // there should be one extra timer event to ensure smooth
+    // transition between multiple blocks
+    REQUIRE(pg.Step(mp) == 0);
+
+    // no pin or position change
+    REQUIRE(st == (ReadPin(IDLER_STEP_PIN) == Level::high));
+    REQUIRE(pg.Position() == 10);
+}
+
+TEST_CASE("pulse_gen::queue_position", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // enqueue two moves, observing Position and CurPosition.
+    REQUIRE(pg.Position() == 0);
+    REQUIRE(pg.CurPosition() == 0);
+
+    // while enqueuing Position should move but CurPosition should not
+    pg.PlanMoveTo(10, 10);
+    REQUIRE(pg.Position() == 10);
+    REQUIRE(pg.CurPosition() == 0);
+
+    pg.PlanMoveTo(15, 10);
+    REQUIRE(pg.Position() == 15);
+    REQUIRE(pg.CurPosition() == 0);
+
+    // step through the moves manually, cycling through two blocks
+    for (size_t i = 0; i != 15; ++i) {
+        REQUIRE((pos_t)i == pg.CurPosition());
+        REQUIRE(pg.Position() == 15);
+        pg.Step(mp);
+    }
+
+    // the final positions should match
+    REQUIRE(pg.CurPosition() == pg.Position());
+}
+
+TEST_CASE("pulse_gen::queue_size", "[pulse_gen]") {
+    PulseGen pg(10, 100);
+
+    // queue should start empty
+    REQUIRE(pg.QueueEmpty());
+
+    // queue the first move
+    CHECK(pg.PlanMoveTo(10, 1));
+    REQUIRE(!pg.QueueEmpty());
+
+    // queue a second move to fill the queue
+    CHECK(pg.PlanMoveTo(15, 1));
+    REQUIRE(pg.Full());
+
+    // further enqueuing should fail
+    REQUIRE(!pg.PlanMoveTo(20, 1));
+}
+
+TEST_CASE("pulse_gen::queue_dropsegments", "[pulse_gen]") {
+    PulseGen pg(10, 100);
+
+    // queue should start empty
+    REQUIRE(pg.QueueEmpty());
+    REQUIRE(pg.Position() == 0);
+
+    // ensure we can enqueue a zero-lenght move successfully
+    REQUIRE(pg.PlanMoveTo(0, 1));
+
+    // however the move shouldn't result in a block entry
+    REQUIRE(pg.QueueEmpty());
+}
+
+TEST_CASE("pulse_gen::queue_step", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // queue should start empty
+    REQUIRE(pg.QueueEmpty());
+
+    // enqueue two moves
+    REQUIRE(pg.PlanMoveTo(15, 1));
+    REQUIRE(pg.PlanMoveTo(5, 1));
+
+    // check for a total lenght of 25 steps (15+(15-5))
+    REQUIRE(stepUntilDone(pg, mp) == 25);
+
+    // check final position
+    REQUIRE(pg.Position() == 5);
+}
+
+TEST_CASE("pulse_gen::queue_abort", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    PulseGen pg(10, 100);
+
+    // queue should start empty
+    REQUIRE(pg.QueueEmpty());
+
+    // enqueue a move and step ~1/3 through
+    REQUIRE(pg.PlanMoveTo(10, 1));
+    REQUIRE(stepUntilDone(pg, mp, 3) == -1);
+
+    // abort the queue
+    pg.AbortPlannedMoves();
+    REQUIRE(pg.QueueEmpty());
+
+    // step shouldn't perform extra moves and return a zero timer
+    bool st = ReadPin(IDLER_STEP_PIN) == Level::high;
+    REQUIRE(pg.Step(mp) == 0);
+    REQUIRE(st == (ReadPin(IDLER_STEP_PIN) == Level::high));
+
+    // check that the aborted position matches
+    REQUIRE(pg.Position() == 3);
+}
+
+TEST_CASE("pulse_gen::accel_ramp", "[pulse_gen]") {
+    MotorParams mp = {
+        .idx = 0,
+        .dirOn = config::idler.dirOn,
+        .csPin = IDLER_CS_PIN,
+        .stepPin = IDLER_STEP_PIN,
+        .sgPin = IDLER_SG_PIN,
+        .uSteps = config::idler.uSteps
+    };
+
+    // TODO: output ramps still to be checked
+    for (int accel = 100; accel <= 5000; accel *= 2) {
+        PulseGen pg(10, accel);
+        pg.PlanMoveTo(100000, 10000);
+
+        unsigned long ts = 0;
+        st_timer_t next;
+        do {
+            next = pg.Step(mp);
+            printf("%lu %u\n", ts, next);
+            ts += next;
+        } while (next);
+
+        printf("\n\n");
+    }
+}
--- a/tests/unit/modules/speed_table/CMakeLists.txt
+++ b/tests/unit/modules/speed_table/CMakeLists.txt
@ -0,0 +1,10 @@
+# define the test executable
+add_executable(
+  speed_table_tests ${CMAKE_SOURCE_DIR}/src/modules/speed_table.cpp test_speed_table.cpp
+  )
+
+# define required search paths
+target_include_directories(speed_table_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules)
+
+# tell build system about the test case
+add_catch_test(speed_table_tests)
--- a/tests/unit/modules/speed_table/test_speed_table.cpp
+++ b/tests/unit/modules/speed_table/test_speed_table.cpp
@ -0,0 +1,111 @@
+#include "catch2/catch.hpp"
+#include "speed_table.h"
+#include <stdio.h>
+
+using Catch::Matchers::Equals;
+using namespace modules::speed_table;
+
+// The following reference values are calculated for 2MHz timer
+static_assert(F_CPU / config::stepTimerFrequencyDivider == 2000000,
+    "speed tables not compatible for the requested frequency");
+
+static const st_timer_t reference[][3] = {
+    { 1, 62500, 1 },
+    { 501, 3992, 1 },
+    { 1001, 1998, 1 },
+    { 1501, 1332, 1 },
+    { 2001, 1000, 1 },
+    { 2501, 801, 1 },
+    { 3001, 668, 1 },
+    { 3501, 572, 1 },
+    { 4001, 500, 1 },
+    { 4501, 444, 1 },
+    { 5001, 400, 1 },
+    { 5501, 364, 1 },
+    { 6001, 333, 1 },
+    { 6501, 307, 1 },
+    { 7001, 285, 1 },
+    { 7501, 266, 1 },
+    { 8001, 250, 1 },
+    { 8501, 234, 1 },
+    { 9001, 222, 1 },
+    { 9501, 211, 1 },
+    { 10001, 400, 2 },
+    { 10501, 381, 2 },
+    { 11001, 364, 2 },
+    { 11501, 348, 2 },
+    { 12001, 333, 2 },
+    { 12501, 320, 2 },
+    { 13001, 308, 2 },
+    { 13501, 297, 2 },
+    { 14001, 286, 2 },
+    { 14501, 276, 2 },
+    { 15001, 267, 2 },
+    { 15501, 258, 2 },
+    { 16001, 250, 2 },
+    { 16501, 243, 2 },
+    { 17001, 235, 2 },
+    { 17501, 228, 2 },
+    { 18001, 222, 2 },
+    { 18501, 216, 2 },
+    { 19001, 211, 2 },
+    { 19501, 205, 2 },
+    { 20001, 400, 4 },
+    { 20501, 391, 4 },
+    { 21001, 381, 4 },
+    { 21501, 371, 4 },
+    { 22001, 364, 4 },
+    { 22501, 356, 4 },
+    { 23001, 348, 4 },
+    { 23501, 340, 4 },
+    { 24001, 333, 4 },
+    { 24501, 326, 4 },
+    { 25001, 320, 4 },
+    { 25501, 312, 4 },
+    { 26001, 308, 4 },
+    { 26501, 301, 4 },
+    { 27001, 297, 4 },
+    { 27501, 290, 4 },
+    { 28001, 286, 4 },
+    { 28501, 280, 4 },
+    { 29001, 276, 4 },
+    { 29501, 270, 4 },
+    { 30001, 267, 4 },
+    { 30501, 262, 4 },
+    { 31001, 258, 4 },
+    { 31501, 254, 4 },
+    { 32001, 250, 4 },
+    { 32501, 247, 4 },
+    { 33001, 243, 4 },
+    { 33501, 239, 4 },
+    { 34001, 235, 4 },
+    { 34501, 231, 4 },
+    { 35001, 228, 4 },
+    { 35501, 225, 4 },
+    { 36001, 222, 4 },
+    { 36501, 219, 4 },
+    { 37001, 216, 4 },
+    { 37501, 214, 4 },
+    { 38001, 211, 4 },
+    { 38501, 208, 4 },
+    { 39001, 205, 4 },
+    { 39501, 201, 4 },
+};
+
+TEST_CASE("speed_table::calc_timer", "[speed_table]") {
+    // Check the result values of calc_timer against an AVR reference table
+    for (unsigned i = 0; i != sizeof(reference) / sizeof(*reference); ++i) {
+        st_timer_t step_rate = reference[i][0];
+        uint8_t loops;
+        st_timer_t timer = calc_timer(step_rate, loops);
+
+        // allow +/-1 of difference for rounding between the C and ASM versions
+        REQUIRE(abs((int)timer - (int)reference[i][1]) <= 1);
+
+        // loops should be exact
+        REQUIRE(loops == reference[i][2]);
+
+        // show the table
+        printf("%u %u %u\n", step_rate, timer, loops);
+    }
+}
--- a/tests/unit/modules/stubs/stub_shr16.cpp
+++ b/tests/unit/modules/stubs/stub_shr16.cpp
@ -6,20 +6,29 @@ namespace shr16 {
 SHR16 shr16;

 uint16_t shr16_v_copy;
+uint8_t shr16_tmc_dir;

 void SHR16::Init() {
    shr16_v_copy = 0;
+    shr16_tmc_dir = 0;
 }

 void SHR16::SetLED(uint16_t led) {
    shr16_v_copy = ((led & 0x00ff) << 8) | ((led & 0x0300) >> 2);
 }
+
 void SHR16::SetTMCEnabled(uint8_t index, bool ena) {
    // do nothing right now
 }
+
 void SHR16::SetTMCDir(uint8_t index, bool dir) {
-    // do nothing right now
+    // this is using another array for testing convenience
+    if (dir)
+        shr16_tmc_dir |= (1 << index);
+    else
+        shr16_tmc_dir &= ~(1 << index);
 }
+
 void SHR16::Write(uint16_t v) {
    // do nothing right now
 }
--- a/tests/unit/modules/stubs/stub_tmc2130.cpp
+++ b/tests/unit/modules/stubs/stub_tmc2130.cpp
@ -0,0 +1,17 @@
+#include "tmc2130.h"
+
+namespace hal {
+namespace tmc2130 {
+
+TMC2130::TMC2130(const MotorParams &params,
+    const MotorCurrents &currents,
+    MotorMode mode) {
+    // TODO
+}
+
+void TMC2130::SetMode(MotorMode mode) {
+    // TODO
+}
+
+} // namespace tmc2130
+} // namespace hal