diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h
index 12b3352..b40505c 100755
--- a/Firmware/include/StepGen2.h
+++ b/Firmware/include/StepGen2.h
@@ -21,7 +21,7 @@ public:
     HardwareTimer *startTimer; // 10,11,13,14
     uint8_t dirPin;
     PinName stepPin;
-    const float Tjitter = 50.0; // Time unit is microseconds
+    const float Tjitter = 5.0; // Time unit is microseconds
 
 public:
     volatile double_t commandedPosition;    // End position when this cycle is completed
diff --git a/Firmware/lib/soes/ecat_slv.c b/Firmware/lib/soes/ecat_slv.c
index 147a11a..e9c57d8 100755
--- a/Firmware/lib/soes/ecat_slv.c
+++ b/Firmware/lib/soes/ecat_slv.c
@@ -191,7 +191,7 @@ void APP_setwatchdog (int watchdogcnt)
  * write ethercat inputs. Implement watch-dog counter to count-out if we have
  * made state change affecting the App.state.
  */
-void DIG_process (uint8_t flags)
+void DIG_process (uint16_t ALEvent, uint8_t flags)
 {
    /* Handle watchdog */
    if((flags & DIG_PROCESS_WD_FLAG) > 0)
@@ -217,14 +217,14 @@ void DIG_process (uint8_t flags)
    if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
    {
       if(((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
-         (ESCvar.ALevent & ESCREG_ALEVENT_SM2))
+         (ALEvent & ESCREG_ALEVENT_SM2))
       {
          RXPDO_update();
          CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
          /* Set outputs */
          cb_set_outputs();
       }
-      else if (ESCvar.ALevent & ESCREG_ALEVENT_SM2)
+      else if (ALEvent & ESCREG_ALEVENT_SM2)
       {
          RXPDO_update();
       }
@@ -339,7 +339,7 @@ void ecat_slv_poll (void)
 void ecat_slv (void)
 {
    ecat_slv_poll();
-   DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+   DIG_process(ESCvar.ALevent, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
          DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
 }
 
diff --git a/Firmware/lib/soes/ecat_slv.h b/Firmware/lib/soes/ecat_slv.h
index c6053c0..b5d0d9d 100755
--- a/Firmware/lib/soes/ecat_slv.h
+++ b/Firmware/lib/soes/ecat_slv.h
@@ -36,7 +36,7 @@ void APP_setwatchdog (int watchdogcnt);
  *
  * @param[in]   flags     = User input what to execute
  */
-void DIG_process (uint8_t flags);
+void DIG_process (uint16_t ALEvent, uint8_t flags);
 
 /**
  * Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 2ca9075..0ecba70 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -8,7 +8,7 @@ extern "C"
 _Objects Obj;
 
 HardwareSerial Serial1(PA10, PA9);
-
+volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
 #define DEBUG_TIM8 1
 #include "MyEncoder.h"
 void indexPulseEncoderCB1(void);
@@ -33,9 +33,10 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
    Encoder1.setLatch(Obj.IndexLatchEnable);
    Encoder1.setScale(Obj.EncPosScale);
 }
-
+volatile uint32_t cmt;
 void handleStepper(void)
 {
+   digitalWrite(Step.dirPin, cmt++ % 2);
    Step.enabled = true;
    Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
    Obj.StepGenOut1.ActualPosition = Step.commandedPosition;
@@ -66,7 +67,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    }
    thenTime = irqTime;
    Obj.DiffT = max_Tim - min_Tim; // Debug
-   Obj.DiffT = ESCvar.ALevent;
+   Obj.DiffT = ALEventIRQ;
+   //Obj.DiffT = Step.frequency;
 }
 
 void ESC_interrupt_enable(uint32_t mask);
@@ -109,7 +111,7 @@ void loop(void)
    if (serveIRQ)
    {
       CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
-      DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+      DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
                   DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
       serveIRQ = 0;
       ESCvar.PrevTime = ESCvar.Time;
@@ -118,14 +120,12 @@ void loop(void)
    if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt
       ecat_slv_poll();
 }
-volatile uint32_t cmt;
+
 void sync0Handler(void)
 {
-   uint32_t lTime;
    irqTime = micros();
    serveIRQ = 1;
-   ESC_read(ESCREG_LOCALTIME, (void *)&lTime, sizeof(lTime)); // Careful! Reads and writes update ALevent also.
-   digitalWrite(Step.dirPin, cmt++ % 2);
+   ALEventIRQ = ESC_ALeventread();
 }
 
 void ESC_interrupt_enable(uint32_t mask)