diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h
index 0404e17..05e1216 100755
--- a/Firmware/include/StepGen2.h
+++ b/Firmware/include/StepGen2.h
@@ -10,19 +10,14 @@ private:
     volatile int32_t nSteps;
     volatile uint32_t timerPulseSteps;
     volatile float Tstart;
-    volatile float Tstop;
-    volatile float Tstep;
     const float maxAllowedFrequency = 100000; // 100 kHz for now
     HardwareTimer *pulseTimer;
     uint32_t pulseTimerChan;
     HardwareTimer *startTimer; // 10,11,13,14
     uint8_t dirPin;
     PinName stepPin;
-    const uint32_t maxFreq = 100000;
     const float Tjitter = 50.0; // Time unit is microseconds
 
-    uint32_t err = 0;
-
 public:
     volatile double_t commandedPosition;    // End position when this cycle is completed
     volatile int32_t commandedStepPosition; // End step position when this cycle is completed
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 00088b7..27ec82e 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -17,21 +17,7 @@ void indexPulseEncoderCB1(void)
 {
    Encoder1.indexPulse();
 }
-#if 0
-#include "StepGen.h"
-void timerCallbackStep1(void);
-StepGen Step1(TIM1, 4, PA_11, PA12, timerCallbackStep1);
-void timerCallbackStep1(void)
-{
-   Step1.timerCB();
-}
-void timerCallbackStep2(void);
-StepGen Step2(TIM3, 4, PC_9, PC10, timerCallbackStep2);
-void timerCallbackStep2(void)
-{
-   Step2.timerCB();
-}
-#endif
+
 #include "StepGen2.h"
 void pulseTimerCallback(void);
 void startTimerCallback(void);
@@ -45,25 +31,15 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
 {
    Encoder1.setLatch(Obj.IndexLatchEnable);
    Encoder1.setScale(Obj.EncPosScale);
-#if 0
-   Step1.reqPos(Obj.StepGenIn1.CommandedPosition);
-   Step1.setScale(Obj.StepGenIn1.StepsPerMM);
-   Step1.enable(Obj.Enable1);
-   Step2.reqPos(Obj.StepGenIn2.CommandedPosition);
-   Step2.setScale(Obj.StepGenIn2.StepsPerMM);
-   Step2.enable(Obj.Enable1);
-#endif
 }
 
 void handleStepper(void)
 {
    Step.enabled = true;
    Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
+   Obj.StepGenOut1.ActualPosition = Step.commandedPosition;
 
-#if 0
-   Step1.handleStepper();
-   Step2.handleStepper();
-#endif
+   Step.handleStepper();
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -72,10 +48,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    Obj.EncPos = Encoder1.currentPos();
    Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
    Obj.IndexByte = Encoder1.getIndexState();
-#if 0
-   Obj.StepGenOut1.ActualPosition = Step1.actPos();
-   Obj.StepGenOut2.ActualPosition = Step2.actPos();
-#endif
+
    uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
    Tim.push(dTim);
    uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
@@ -189,9 +162,6 @@ uint16_t dc_checker(void)
 {
    // Indicate we run DC
    ESCvar.dcsync = 1;
-#if 0
-   StepGen::sync0CycleTime = ESC_SYNC0cycletime() / 1000;  // usecs
-#endif
    StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
    return 0;
 }