diff --git a/Firmware/include/StepGen.h b/Firmware/include/StepGen.h
index eeeb469..0a5cbf5 100755
--- a/Firmware/include/StepGen.h
+++ b/Firmware/include/StepGen.h
@@ -23,11 +23,11 @@ public:
     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);
-    double actPos();
+    void reqPos(double_t pos);
     double reqPos();
+    void actPos(double_t pos);
+    double actPos();
     void handleStepper(void);
-    void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/);
     void timerCB();
     void setScale(int32_t spm);
 };
diff --git a/Firmware/src/StepGen.cpp b/Firmware/src/StepGen.cpp
index 7fc8122..264aa29 100755
--- a/Firmware/src/StepGen.cpp
+++ b/Firmware/src/StepGen.cpp
@@ -30,57 +30,59 @@ StepGen::StepGen(TIM_TypeDef *Timer, uint8_t _timerChannel, uint8_t _stepPin, ui
             pinMode(STEPPER_DIR_PIN, OUTPUT);
       */
 }
-void StepGen::cmdPos(double_t pos)
+void StepGen::reqPos(double_t pos)
 {
     requestedPosition = pos;
 }
-
-double StepGen::actPos()
-{
-    return actualPosition;
-}
 double StepGen::reqPos()
 {
     return requestedPosition;
 }
+void StepGen::actPos(double pos)
+{
+    actualPosition = pos;
+}
+double StepGen::actPos()
+{
+    return actualPosition;
+}
+
 void StepGen::handleStepper(void)
 {
-    actualPosition = timerStepPosition / double(stepsPerMM);
-    double diffPosition = requestedPosition - actualPosition;
+    actPos(timerStepPosition / double(stepsPerMM));
+    double diffPosition = reqPos() - actPos();
 
     uint64_t fre = abs(diffPosition) * stepsPerMM * 1000000 / double(sync0CycleTime); // Frequency needed
     if (fre > maxFreq)                                                                // Only do maxFre
     {
-        double maxDist = maxFreq/stepsPerMM * sync0CycleTime / 1000000.0 * (diffPosition > 0 ? 1 : -1);
-        requestedPosition = actualPosition + maxDist;
+        double maxDist = maxFreq / stepsPerMM * sync0CycleTime / 1000000.0 * (diffPosition > 0 ? 1 : -1);
+        reqPos(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
-}
+    int32_t pulsesAtEndOfCycle = stepsPerMM * reqPos(); // From Turner.hal X:5000 Z:2000 ps/mm
+
+    sync0CycleTime = 800;
 
-void StepGen::makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/)
-{
     if (timerIsRunning)
     {
         // Set variables, they will be picked up by the timer_CB and the timer is reloaded.
-        timerNewEndStepPosition = pulsesAtEnd;
-        timerNewCycleTime = cycleTime;
+        timerNewEndStepPosition = pulsesAtEndOfCycle;
+        timerNewCycleTime = sync0CycleTime;
     }
     if (!timerIsRunning)
     {
         // Start the timer
-        int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or -
+        int32_t steps = pulsesAtEndOfCycle - timerStepPositionAtEnd; // Pulses to go + or -
         if (steps != 0)
         {
             uint8_t sgn = steps > 0 ? HIGH : LOW;
             digitalWrite(dirPin, sgn);
-            double_t freqf = (abs(steps) * 1000000.0) / double(cycleTime);
+            double_t freqf = (abs(steps) * 1000000.0) / double(sync0CycleTime);
             uint32_t freq = uint32_t(freqf);
             // freq=1428;
             MyTim->setOverflow(freq, HERTZ_FORMAT);
             MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
             timerStepDirection = steps > 0 ? 1 : -1;
-            timerStepPositionAtEnd = pulsesAtEnd; // Current Position
+            timerStepPositionAtEnd = pulsesAtEndOfCycle; // Current Position
             timerIsRunning = 1;
             MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
             MyTim->resume();
@@ -95,19 +97,17 @@ void StepGen::timerCB()
         // 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();
+        MyTim->pause(); // Does this hurt?
         int32_t steps = timerNewEndStepPosition - timerStepPosition;
         if (steps != 0)
         {
             uint8_t sgn = steps > 0 ? HIGH : LOW;
             digitalWrite(stepPin, sgn);
-            double_t freqf = (abs(steps) * 1000000.0) / double(timerNewCycleTime);
+            double_t freqf = abs(steps) * (1e6 / double(timerNewCycleTime));
             uint32_t freq = uint32_t(freqf);
-            // freq=1428;
             if (freq != 0)
             {
                 MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
-                // freq=1428;
                 MyTim->setOverflow(freq, HERTZ_FORMAT);
                 MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
                 timerStepDirection = steps > 0 ? 1 : -1;
diff --git a/Firmware/src/Stm32F4_Encoder.cpp b/Firmware/src/Stm32F4_Encoder.cpp
index eba7c4f..dbe7226 100755
--- a/Firmware/src/Stm32F4_Encoder.cpp
+++ b/Firmware/src/Stm32F4_Encoder.cpp
@@ -291,7 +291,7 @@ void rcc_config()
 
     GPIO_PinAF(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
     GPIO_PinAF(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
-
+#if 0 // Skipping since I use TIM8 as stepper generator
     TIM_EncoderInterConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
     TIMER_InitStructure.TIM_Period = 65535;
     TIMER_InitStructure.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
@@ -299,7 +299,7 @@ void rcc_config()
     TIM_TimeBaseStructInit(&TIMER_InitStructure);
     TIM_Cmd(TIM8, ENABLE);
     TIM8->CNT = 0;
-
+#endif
     TIM_EncoderInterConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
     TIMER_InitStructureE.TIM_Period = 65535;
     TIMER_InitStructureE.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 7f9dccc..9fc4baa 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -44,8 +44,8 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
    Encoder1.setLatch(Obj.IndexLatchEnable);
    Encoder1.setScale(Obj.EncPosScale);
 
-   Step1.cmdPos(Obj.StepGenIn1.CommandedPosition);
-   Step2.cmdPos(Obj.StepGenIn2.CommandedPosition);
+   Step1.reqPos(Obj.StepGenIn1.CommandedPosition);
+   Step2.reqPos(Obj.StepGenIn2.CommandedPosition);
 }
 
 void handleStepper(void)
@@ -96,7 +96,7 @@ volatile byte serveIRQ = 0;
 void setup(void)
 {
    Serial1.begin(115200);
-   rcc_config();
+   rcc_config(); // probably breaks some timers.
 
    Step1.setScale(1250); // 2000 /rev 4 mm/rev x 2.5 gear => 1250 /mm
    Step2.setScale(500);  // 2000 /rev 4 mm/rev => 500 /mm
diff --git a/Kicad/Ethercat-stm32/Ethercat-stm32.kicad_prl b/Kicad/Ethercat-stm32/Ethercat-stm32.kicad_prl
index 727f528..48b50a2 100755
--- a/Kicad/Ethercat-stm32/Ethercat-stm32.kicad_prl
+++ b/Kicad/Ethercat-stm32/Ethercat-stm32.kicad_prl
@@ -64,7 +64,7 @@
       39,
       40
     ],
-    "visible_layers": "002202a_00000001",
+    "visible_layers": "002202b_80000005",
     "zone_display_mode": 0
   },
   "meta": {