diff --git a/Firmware/include/StepGen.h b/Firmware/include/StepGen.h
index 2b88172..095e5fd 100755
--- a/Firmware/include/StepGen.h
+++ b/Firmware/include/StepGen.h
@@ -15,11 +15,12 @@ public:
     volatile double_t actualPosition;
     volatile double_t requestedPosition;
     HardwareTimer *MyTim;
-    const uint32_t stepsPerMM = 1000;
+    uint32_t stepsPerMM;
     uint32_t sync0CycleTime;
     uint8_t dirPin;
     uint8_t stepPin;
     uint8_t timerChan;
+    const uint32_t maxFreq = 100000;
 
     StepGen(TIM_TypeDef *Timer, uint8_t timerChannel, uint8_t stepPin, uint8_t dirPin, void irq(void));
     void cmdPos(double_t pos);
@@ -29,6 +30,7 @@ public:
     void setCycleTime(uint32_t cycleTime);
     void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/);
     void timerCB();
+    void setScale(int32_t spm);
 };
 
 #endif
\ No newline at end of file
diff --git a/Firmware/src/StepGen.cpp b/Firmware/src/StepGen.cpp
index 665a75e..8bfa1de 100755
--- a/Firmware/src/StepGen.cpp
+++ b/Firmware/src/StepGen.cpp
@@ -12,6 +12,7 @@ StepGen::StepGen(TIM_TypeDef *Timer, uint8_t _timerChannel, uint8_t _stepPin, ui
     timerNewCycleTime = 0;
     actualPosition = 0;
     requestedPosition = 0;
+    stepsPerMM = 0;
 
     dirPin = _dirPin;
     stepPin = _stepPin;
@@ -46,9 +47,12 @@ void StepGen::handleStepper(void)
 {
     actualPosition = timerStepPosition / double(stepsPerMM);
     double diffPosition = requestedPosition - actualPosition;
-    if (abs(diffPosition) * stepsPerMM > 10000)
+
+    uint64_t fre = abs(diffPosition) * stepsPerMM * 1000000 / double(sync0CycleTime); // Frequency needed
+    if (fre > maxFreq)                                                                // Only do maxFre
     {
-        requestedPosition = actualPosition + 10.0 * (diffPosition > 0 ? 1 : -1);
+        double maxDist = maxFreq/stepsPerMM * sync0CycleTime / 1000000.0 * (diffPosition > 0 ? 1 : -1);
+        requestedPosition = actualPosition + maxDist;
     }
     int32_t pulsesAtEndOfCycle = stepsPerMM * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm
     makePulses(sync0CycleTime, pulsesAtEndOfCycle);              // Make the pulses using hardware timer
@@ -60,7 +64,6 @@ void StepGen::setCycleTime(uint32_t cycleTime)
 
 void StepGen::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.
@@ -126,3 +129,8 @@ void StepGen::timerCB()
         MyTim->pause();
     }
 }
+
+void StepGen::setScale(int32_t spm)
+{
+    stepsPerMM = spm;
+}
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 8549329..919802d 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -125,13 +125,9 @@ void setup(void)
 {
    Serial1.begin(115200);
    rcc_config();
-#if 0
-   TIM_TypeDef *Instance = TIM1;
-   MyTim = new HardwareTimer(Instance);
-   MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN);
-   MyTim->attachInterrupt(TimerStep_CB);
-   pinMode(STEPPER_DIR_PIN, OUTPUT);
-#endif
+
+   Step1.setScale(100);
+
    // Set starting count value
    EncoderInit.SetCount(Tim2, 0);
    attachInterrupt(digitalPinToInterrupt(INDEX_PIN), indexPulse, RISING); // When Index triggered
@@ -174,18 +170,6 @@ void sync0Handler(void)
 void handleStepper(void)
 {
    Step1.handleStepper();
-#if 0
-
-   const uint32_t steps_per_mm = 1000;
-   actualPosition = timerStepPosition / double(steps_per_mm);
-   double diffPosition = requestedPosition - actualPosition;
-   if (abs(diffPosition) * steps_per_mm > 10000)
-   {
-      requestedPosition = actualPosition + 10.0 * (diffPosition > 0 ? 1 : -1);
-   }
-   int32_t pulsesAtEndOfCycle = steps_per_mm * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm
-   makePulses(sync0CycleTime, pulsesAtEndOfCycle);                // Make the pulses using hardware timer
-#endif
 }
 
 void ESC_interrupt_enable(uint32_t mask)