diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp
index 5befadc..8c666cd 100755
--- a/Firmware/src/StepGen2.cpp
+++ b/Firmware/src/StepGen2.cpp
@@ -29,31 +29,37 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
 
 uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
 {
-    if (!enabled)
+    frequency = 0;
+    nSteps = 0;
+    dbg=0;
+    if (!enabled) // Just .... don't
         return updatePos(0);
 
-    lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9;    // // nLoops is there in case we missed a ethercat cycle. secs
     commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
     if (initialStepPosition == commandedStepPosition)              // No movement
+    {
+
         return updatePos(1);
+    }
 
     nSteps = commandedStepPosition - initialStepPosition;
+    lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
 
-    if (abs(nSteps) < 1) // Some small number
-    {
-        frequency = (abs(nSteps) + 1) / lcncCycleTime;
-        Tpulses = abs(nSteps) / frequency;
-        Tstartf = (lcncCycleTime - Tpulses) / 2.0;
-    }
-    else // Regular step train, up or down
-    {
-        float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation.  position = kTRAJ x time + mTRAJ
-        float mTRAJ = initialPosition;                                       // Operating on incoming positions (not steps)
-        if (kTRAJ > 0)
-            Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ;
-        else
-            Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;
-        frequency = fabs(kTRAJ * stepsPerMM); //
+    if (abs(nSteps) < 1)                                                                    // Some small number
+    {                                                                                       //
+        frequency = (abs(nSteps) + 1) / lcncCycleTime;                                      // Distribute steps inside available time
+        Tpulses = abs(nSteps) / frequency;                                                  //
+        Tstartf = (lcncCycleTime - Tpulses) / 2.0;                                          //
+    }                                                                                       //
+    else                                                                                    // Regular step train, up or down
+    {                                                                                       //
+        float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime;                // Straight line equation.  position = kTRAJ x time + mTRAJ
+        float mTRAJ = initialPosition;                                                      // Operating on incoming positions (not steps)
+        if (kTRAJ > 0)                                                                      //
+            Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
+        else                                                                                //
+            Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;     // Crossing downwards
+        frequency = fabs(kTRAJ * stepsPerMM);                                               //
         Tpulses = abs(nSteps) / frequency;
     }
     updatePos(5);
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index ab9fea1..960e2dd 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -68,12 +68,12 @@ void handleStepper(void)
    Step1.enabled = true;
    Step1.commandedPosition = Obj.CommandedPosition1;
    Step1.stepsPerMM = Obj.StepsPerMM1;
-   Step1.handleStepper(irqTime, nLoops);
+   Step1.handleStepper(irqTime, 1/*nLoops*/);
 
    Step2.enabled = true;
    Step2.commandedPosition = Obj.CommandedPosition2;
    Step2.stepsPerMM = Obj.StepsPerMM2;
-   Step2.handleStepper(irqTime, nLoops);
+   Step2.handleStepper(irqTime, 1/*nLoops*/);
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -95,10 +95,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
          min_Tim = aTim;
    }
    thenTime = irqTime;
-   Obj.DiffT = max_Tim - min_Tim; // Debug
+   Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
    Obj.D1 = Step2.frequency;
-   Obj.D2 = Step2.Tstartf * 1e6;
-   Obj.D3 = Step2.dbg;
+   Obj.D2 = abs(Step2.nSteps);
+   Obj.D3 = Step2.Tstartu;
    Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
 }