diff --git a/Firmware/include/StepGen.h b/Firmware/include/StepGen.h
new file mode 100755
index 0000000..3fac40d
--- /dev/null
+++ b/Firmware/include/StepGen.h
@@ -0,0 +1,145 @@
+
+#ifndef STEPGEN
+#define STEPGEN
+#include <HardwareTimer.h>
+
+class StepGen
+{
+public:
+    volatile uint8_t timerIsRunning;
+    volatile int32_t timerStepPosition;
+    volatile int32_t timerStepDirection;
+    volatile int32_t timerStepPositionAtEnd;
+    volatile int32_t timerNewEndStepPosition;
+    volatile uint32_t timerNewCycleTime;
+    volatile double_t actualPosition;
+    volatile double_t requestedPosition;
+    HardwareTimer *MyTim;
+    const uint32_t stepsPerMM = 1000;
+    uint32_t sync0CycleTime;
+
+    StepGen(TIM_TypeDef *Timer, uint8_t timerChannel, uint8_t stepPin, uint8_t dirPin, void irq(void))
+    {
+        timerIsRunning = 0;
+        timerStepPosition = 0;
+        timerStepDirection = 0;
+        timerStepPositionAtEnd = 0;
+        timerNewEndStepPosition = 0;
+        timerNewCycleTime = 0;
+        actualPosition = 0;
+        requestedPosition = 0;
+
+        MyTim = new HardwareTimer(Timer);
+        MyTim->setMode(timerChannel, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
+        MyTim->attachInterrupt(irq);
+        pinMode(dirPin, OUTPUT);
+
+        /*
+                TIM_TypeDef *Timer = TIM1;
+                MyTim = new HardwareTimer(Timer);
+                MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
+                MyTim->attachInterrupt(TimerStep_CB);
+                pinMode(STEPPER_DIR_PIN, OUTPUT);
+          */
+    }
+
+    void cmdPos(double_t pos)
+    {
+        requestedPosition = pos;
+    }
+
+    double actPos()
+    {
+        return actualPosition;
+    }
+    double reqPos()
+    {
+        return requestedPosition;
+    }
+    void handleStepper(void)
+    {
+        actualPosition = timerStepPosition / double(stepsPerMM);
+        double diffPosition = requestedPosition - actualPosition;
+        if (abs(diffPosition) * stepsPerMM > 10000)
+        {
+            requestedPosition = actualPosition + 10.0 * (diffPosition > 0 ? 1 : -1);
+        }
+        int32_t pulsesAtEndOfCycle = stepsPerMM * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm
+        makePulses(sync0CycleTime, pulsesAtEndOfCycle);              // Make the pulses using hardware timer
+    }
+    void setCycleTime(uint32_t cycleTime)
+    {
+        sync0CycleTime = cycleTime;
+    }
+
+    void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/)
+    {
+        uint32_t now = micros();
+        if (timerIsRunning)
+        {
+            // Set variables, they will be picked up by the timer_CB and the timer is reloaded.
+            timerNewEndStepPosition = pulsesAtEnd;
+            timerNewCycleTime = cycleTime;
+        }
+        if (!timerIsRunning)
+        {
+            // Start the timer
+            int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or -
+            if (steps != 0)
+            {
+                uint8_t sgn = steps > 0 ? HIGH : LOW;
+                digitalWrite(STEPPER_DIR_PIN, sgn);
+                double_t freqf = (abs(steps) * 1000000.0) / double(cycleTime);
+                uint32_t freq = uint32_t(freqf);
+                // freq=1428;
+                MyTim->setOverflow(freq, HERTZ_FORMAT);
+                MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 %
+                timerStepDirection = steps > 0 ? 1 : -1;
+                timerStepPositionAtEnd = pulsesAtEnd; // Current Position
+                timerIsRunning = 1;
+                MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
+                MyTim->resume();
+            }
+        }
+    }
+    void timerCB()
+    {
+        timerStepPosition += timerStepDirection; // The step that was just completed
+        if (timerNewCycleTime != 0)              // Are we going to reload?
+        {
+            // Input for reload is timerNewEndStepPosition and timerNewEndTime
+            // The timer has current position and current time and from this
+            // can set new frequency and new endtarget for steps
+            MyTim->pause();
+            int32_t steps = timerNewEndStepPosition - timerStepPosition;
+            if (steps != 0)
+            {
+                uint8_t sgn = steps > 0 ? HIGH : LOW;
+                digitalWrite(STEPPER_DIR_PIN, sgn);
+                double_t freqf = (abs(steps) * 1000000.0) / double(timerNewCycleTime);
+                uint32_t freq = uint32_t(freqf);
+                // freq=1428;
+                if (freq != 0)
+                {
+                    MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
+                    // freq=1428;
+                    MyTim->setOverflow(freq, HERTZ_FORMAT);
+                    MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 %
+                    timerStepDirection = steps > 0 ? 1 : -1;
+                    timerStepPositionAtEnd = timerNewEndStepPosition;
+                    timerNewEndStepPosition = 0; // Set to zero to not reload next time
+                    timerNewCycleTime = 0;
+                    MyTim->resume();
+                    timerIsRunning = 1;
+                }
+            }
+        }
+        if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
+        {
+            timerIsRunning = 0;
+            MyTim->pause();
+        }
+    }
+};
+
+#endif
\ No newline at end of file
diff --git a/Firmware/src/StepGen.cpp b/Firmware/src/StepGen.cpp
new file mode 100755
index 0000000..e69de29