diff --git a/Firmware/include/StepGen3.h b/Firmware/include/StepGen3.h
index 7247dcb..ca3f52e 100755
--- a/Firmware/include/StepGen3.h
+++ b/Firmware/include/StepGen3.h
@@ -146,7 +146,7 @@ private:
 };
 
 // For the example
-#define BASE_PERIOD 40000 
+#define BASE_PERIOD 20000 
 #define SERVO_PERIOD 2000000
 #define JOINT_X_STEPGEN_MAXACCEL 0.0
 #define JOINT_Z_STEPGEN_MAXACCEL 0.0
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 1aa63d6..b10e072 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -62,11 +62,11 @@ void syncWithLCNC()
    deltaSyncTime = syncTime - prevSyncTime;
    deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt;
    prevMakePulsesCnt = makePulsesCnt;
-   Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions
-   Step->makeAllPulses();                   // Make first step right here
-   basePeriodCnt = SERVO_PERIOD / BASE_PERIOD;   //
-   baseTimer->refresh();                    //
-   baseTimer->resume();                     // Make the other steps in ISR
+   Step->updateStepGen(pos_cmd1, pos_cmd2);    // Update positions
+   Step->makeAllPulses();                      // Make first step right here
+   basePeriodCnt = SERVO_PERIOD / BASE_PERIOD; //
+   baseTimer->refresh();                       //
+   baseTimer->resume();                        // Make the other steps in ISR
    // baseTimer->isRunning();
 }
 
@@ -220,7 +220,7 @@ void setup(void)
    baseTimer = new HardwareTimer(TIM11); // The base period timer
    uint32_t usecs = BASE_PERIOD / sync0CycleTime;
    // setFrequencyAdjustedMicrosSeconds(baseTimer, usecs);
-   baseTimer->setOverflow(35, MICROSEC_FORMAT);
+   baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT);
    baseTimer->attachInterrupt(basePeriodCB);
 
    syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer