diff --git a/Firmware/include/MyEncoder.h b/Firmware/include/MyEncoder.h
index cf579bc..e892067 100755
--- a/Firmware/include/MyEncoder.h
+++ b/Firmware/include/MyEncoder.h
@@ -3,16 +3,16 @@
 #include "Stm32F4_Encoder.h"
 #include <CircularBuffer.h>
 #define RINGBUFFERLEN 101
-   //  EncoderInit.SetCount(Tim3, 0);
-   //  EncoderInit.SetCount(Tim4, 0);
-   //  EncoderInit.SetCount(Tim8, 0);
+//  EncoderInit.SetCount(Tim3, 0);
+//  EncoderInit.SetCount(Tim4, 0);
+//  EncoderInit.SetCount(Tim8, 0);
 class MyEncoder
 {
 public:
     int32_t previousEncoderCounterValue = 0;
     double PosScaleRes = 1.0;
     uint32_t CurPosScale = 1;
-    uint8_t OldLatchCEnable = 0;
+    uint8_t oldLatchCEnable = 0;
     volatile uint8_t indexPulseFired = 0;
     volatile uint8_t pleaseZeroTheCounter = 0;
     Encoder EncoderInit;
@@ -22,15 +22,19 @@ public:
     CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
     double curPos;
 
+    TIM_TypeDef *tim_base;
+
 public:
     MyEncoder(uint8_t _indexPin, void irq(void));
     int32_t unwrapEncoder(uint16_t in);
     void indexPulse(void);
-    void init(enum EncTimer timer);
+    void init(enum EncTimer timer, TIM_TypeDef *_tim_base);
     uint8_t indexHappened();
-    double currentPos();
+    double currentPos(volatile uint32_t cnt);
     double frequency(uint64_t time);
     uint8_t getIndexState();
+    void setScale(double scale);
+    void setLatch(uint8_t latchEnable);
 };
 
 #endif
diff --git a/Firmware/src/MyEncoder.cpp b/Firmware/src/MyEncoder.cpp
index 2207c79..8bfd576 100755
--- a/Firmware/src/MyEncoder.cpp
+++ b/Firmware/src/MyEncoder.cpp
@@ -36,8 +36,9 @@ void MyEncoder::indexPulse(void)
         pleaseZeroTheCounter = 0;
     }
 }
-void MyEncoder::init(enum EncTimer timer)
+void MyEncoder::init(enum EncTimer timer, TIM_TypeDef *_tim_base)
 {
+    tim_base = _tim_base;
     // Set starting count value
     EncoderInit.SetCount(timer, 0);
     // EncoderInit.SetCount(Tim3, 0);
@@ -56,9 +57,9 @@ uint8_t MyEncoder::indexHappened()
     return 0;
 }
 
-double MyEncoder::currentPos()
+double MyEncoder::currentPos(volatile uint32_t cnt)
 {
-    curPos = unwrapEncoder(TIM2->CNT) * PosScaleRes;
+    curPos = unwrapEncoder(tim_base->CNT) * PosScaleRes;
     return curPos;
 }
 
@@ -80,3 +81,21 @@ uint8_t MyEncoder::getIndexState()
 {
     return digitalRead(indexPin);
 }
+
+void MyEncoder::setScale(double scale)
+{
+    if (CurPosScale != scale && scale != 0)
+    {
+        CurPosScale = scale;
+        PosScaleRes = 1.0 / double(scale);
+    }
+}
+
+void MyEncoder::setLatch(uint8_t latchEnable)
+{
+    if (latchEnable && !oldLatchCEnable) // Should only happen first time IndexCEnable is set
+    {
+        pleaseZeroTheCounter = 1;
+    }
+    oldLatchCEnable = latchEnable;
+}
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index ce42784..c496cb8 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -1,15 +1,13 @@
 #include <Arduino.h>
-
 #include <stdio.h>
 extern "C"
 {
 #include "ecat_slv.h"
 #include "utypes.h"
 };
-
+_Objects Obj;
 
 HardwareSerial Serial1(PA10, PA9);
-_Objects Obj;
 
 #include "MyEncoder.h"
 #define INDEX_PIN PA2
@@ -30,27 +28,18 @@ void timerCallbackStep1(void)
 
 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
 {
-   if (Obj.IndexLatchEnable && !Encoder1.OldLatchCEnable) // Should only happen first time IndexCEnable is set
-   {
-      Encoder1.pleaseZeroTheCounter = 1;
-   }
-   Encoder1.OldLatchCEnable = Obj.IndexLatchEnable;
-
-   if (Encoder1.CurPosScale != Obj.EncPosScale && Obj.EncPosScale != 0)
-   {
-      Encoder1.CurPosScale = Obj.EncPosScale;
-      Encoder1.PosScaleRes = 1.0 / double(Encoder1.CurPosScale);
-   }
+   Encoder1.setLatch(Obj.IndexLatchEnable);
+   Encoder1.setScale(Obj.EncPosScale);
    Step1.cmdPos(Obj.StepGenIn1.CommandedPosition);
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
 {
    Obj.IndexStatus = Encoder1.indexHappened();
-   Obj.EncPos = Encoder1.currentPos();
+   Obj.EncPos = Encoder1.currentPos(TIM2->CNT);
    Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
    Obj.IndexByte = Encoder1.getIndexState();
-    if (Obj.IndexByte)
+   if (Obj.IndexByte)
       Serial1.printf("IS 1\n");
    Obj.StepGenOut1.ActualPosition = Step1.actPos();
    Obj.DiffT = 10000 * Step1.reqPos();
@@ -61,7 +50,6 @@ void ESC_interrupt_disable(uint32_t mask);
 uint16_t dc_checker(void);
 void sync0Handler(void);
 void handleStepper(void);
-void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* nr of pulses to do*/);
 
 static esc_cfg_t config =
     {
@@ -91,8 +79,7 @@ void setup(void)
    rcc_config();
 
    Step1.setScale(500);
-   Encoder1.init(Tim2);
-
+   Encoder1.init(Tim2, TIM2);
 
    ecat_slv_init(&config);
 }