diff --git a/src/main.cpp b/src/main.cpp
index 28d5d43..30794ae 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -19,6 +19,7 @@
 #include "modules/selector.h"
 #include "modules/user_input.h"
 #include "modules/timebase.h"
+#include "modules/motion.h"
 
 #include "logic/command_base.h"
 #include "logic/cut_filament.h"
@@ -126,7 +127,7 @@ void setup() {
     ml::leds.SetMode(2, ml::Color::green, ml::Mode::on);
     ml::leds.Step();
 
-    // tmc::Init()
+    mm::Init();
     ml::leds.SetMode(1, ml::Color::green, ml::Mode::on);
     ml::leds.Step();
 
diff --git a/src/modules/CMakeLists.txt b/src/modules/CMakeLists.txt
index 3c5856c..139f1e0 100644
--- a/src/modules/CMakeLists.txt
+++ b/src/modules/CMakeLists.txt
@@ -12,6 +12,7 @@ target_sources(
           movable_base.cpp
           permanent_storage.cpp
           selector.cpp
+          speed_table.cpp
           timebase.cpp
           user_input.cpp
           pulse_gen.cpp
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 58ffb5e..2ee45fa 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -1,5 +1,12 @@
 #include "motion.h"
 
+// TODO: use proper timer abstraction
+#ifdef __AVR__
+#include <avr/interrupt.h>
+#else
+//#include "../hal/timers.h"
+#endif
+
 namespace modules {
 namespace motion {
 
@@ -89,7 +96,51 @@ st_timer_t Motion::Step() {
     return next;
 }
 
-void Isr() {}
+static inline void Isr() {
+    st_timer_t next = motion.Step();
+#ifdef __AVR__
+    // TODO: use proper timer abstraction
+    if (next)
+        OCR1A = next;
+    else {
+        // Idling: plan the next interrupt after 8ms from now.
+        OCR1A = 0x4000;
+    }
+#endif
+}
+
+void Init() {
+#ifdef __AVR__
+    // TODO: use proper timer abstraction
+
+    // waveform generation = 0100 = CTC
+    TCCR1B &= ~(1 << WGM13);
+    TCCR1B |= (1 << WGM12);
+    TCCR1A &= ~(1 << WGM11);
+    TCCR1A &= ~(1 << WGM10);
+
+    // output mode = 00 (disconnected)
+    TCCR1A &= ~(3 << COM1A0);
+    TCCR1A &= ~(3 << COM1B0);
+
+    // Set the timer pre-scaler
+    // We use divider of 8, resulting in a 2MHz timer frequency on a 16MHz MCU
+    TCCR1B = (TCCR1B & ~(0x07 << CS10)) | (2 << CS10);
+
+    // Plan the first interrupt after 8ms from now.
+    OCR1A = 0x4000;
+    TCNT1 = 0;
+
+    // Enable interrupt
+    TIMSK1 |= (1 << OCIE1A);
+#endif
+}
 
 } // namespace motion
 } // namespace modules
+
+#ifdef __AVR__
+ISR(TIMER1_COMPA_vect) {
+    modules::motion::Isr();
+}
+#endif
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 79b8a2c..5fd3e08 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -250,8 +250,8 @@ private:
     };
 };
 
-/// ISR stepping routine
-//extern void Isr();
+/// ISR initialization
+extern void Init();
 
 extern Motion motion;