diff --git a/Firmware/include/MyEncoder.h b/Firmware/include/MyEncoder.h
index 8a537a0..cf579bc 100755
--- a/Firmware/include/MyEncoder.h
+++ b/Firmware/include/MyEncoder.h
@@ -23,86 +23,14 @@ public:
     double curPos;
 
 public:
-    MyEncoder(uint8_t _indexPin, void irq(void))
-    {
-        indexPin = _indexPin;
-        attachInterrupt(digitalPinToInterrupt(_indexPin), irq, RISING); // When Index triggered
-    }
-
-#define ONE_PERIOD 65536
-#define HALF_PERIOD 32768
-
-    int32_t unwrap_encoder(uint16_t in)
-    {
-        int32_t c32 = (int32_t)in - HALF_PERIOD;           // remove half period to determine (+/-) sign of the wrap
-        int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
-
-        // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
-        int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
-        if (dif < -HALF_PERIOD)
-            mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
-
-        int32_t unwrapped = previousEncoderCounterValue + mod_dif;
-        previousEncoderCounterValue = unwrapped; // load previous value
-
-        return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
-    }
-
-    void indexPulse(void)
-    {
-        if (pleaseZeroTheCounter)
-        {
-            TIM2->CNT = 0;
-            indexPulseFired = 1;
-            Pos.clear();
-            TDelta.clear();
-            pleaseZeroTheCounter = 0;
-        }
-    }
-    void init(enum EncTimer timer)
-    {
-        // Set starting count value
-        EncoderInit.SetCount(timer, 0);
-        // EncoderInit.SetCount(Tim3, 0);
-        // EncoderInit.SetCount(Tim4, 0);
-        // EncoderInit.SetCount(Tim8, 0);
-    }
-
-    uint8_t indexHappened()
-    {
-        if (indexPulseFired)
-        {
-            indexPulseFired = 0;
-            previousEncoderCounterValue = 0;
-            return 1;
-        }
-        return 0;
-    }
-
-    double currentPos()
-    {
-        curPos = unwrap_encoder(TIM2->CNT) * PosScaleRes;
-        return curPos;
-    }
-
-    double frequency(uint64_t time)
-    {
-
-        double diffT = 0;
-        double diffPos = 0;
-        TDelta.push(time); // Running average over the length of the circular buffer
-        Pos.push(curPos);
-        if (Pos.size() >= 2)
-        {
-            diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
-            diffPos = fabs(Pos.last() - Pos.first());
-        }
-        return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
-    }
-    uint8_t getIndexState()
-    {
-        return digitalRead(indexPin);
-    }
+    MyEncoder(uint8_t _indexPin, void irq(void));
+    int32_t unwrapEncoder(uint16_t in);
+    void indexPulse(void);
+    void init(enum EncTimer timer);
+    uint8_t indexHappened();
+    double currentPos();
+    double frequency(uint64_t time);
+    uint8_t getIndexState();
 };
 
 #endif
diff --git a/Firmware/src/MyEncoder.cpp b/Firmware/src/MyEncoder.cpp
new file mode 100755
index 0000000..2207c79
--- /dev/null
+++ b/Firmware/src/MyEncoder.cpp
@@ -0,0 +1,82 @@
+#include "MyENcoder.h"
+
+MyEncoder::MyEncoder(uint8_t _indexPin, void irq(void))
+{
+    indexPin = _indexPin;
+    attachInterrupt(digitalPinToInterrupt(indexPin), irq, RISING); // When Index triggered
+}
+
+#define ONE_PERIOD 65536
+#define HALF_PERIOD 32768
+
+int32_t MyEncoder::unwrapEncoder(uint16_t in)
+{
+    int32_t c32 = (int32_t)in - HALF_PERIOD;           // remove half period to determine (+/-) sign of the wrap
+    int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
+
+    // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
+    int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
+    if (dif < -HALF_PERIOD)
+        mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
+
+    int32_t unwrapped = previousEncoderCounterValue + mod_dif;
+    previousEncoderCounterValue = unwrapped; // load previous value
+
+    return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
+}
+
+void MyEncoder::indexPulse(void)
+{
+    if (pleaseZeroTheCounter)
+    {
+        TIM2->CNT = 0;
+        indexPulseFired = 1;
+        Pos.clear();
+        TDelta.clear();
+        pleaseZeroTheCounter = 0;
+    }
+}
+void MyEncoder::init(enum EncTimer timer)
+{
+    // Set starting count value
+    EncoderInit.SetCount(timer, 0);
+    // EncoderInit.SetCount(Tim3, 0);
+    // EncoderInit.SetCount(Tim4, 0);
+    // EncoderInit.SetCount(Tim8, 0);
+}
+
+uint8_t MyEncoder::indexHappened()
+{
+    if (indexPulseFired)
+    {
+        indexPulseFired = 0;
+        previousEncoderCounterValue = 0;
+        return 1;
+    }
+    return 0;
+}
+
+double MyEncoder::currentPos()
+{
+    curPos = unwrapEncoder(TIM2->CNT) * PosScaleRes;
+    return curPos;
+}
+
+double MyEncoder::frequency(uint64_t time)
+{
+
+    double diffT = 0;
+    double diffPos = 0;
+    TDelta.push(time); // Running average over the length of the circular buffer
+    Pos.push(curPos);
+    if (Pos.size() >= 2)
+    {
+        diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
+        diffPos = fabs(Pos.last() - Pos.first());
+    }
+    return diffT != 0 ? diffPos / diffT : 0.0; // Revolutions per second
+}
+uint8_t MyEncoder::getIndexState()
+{
+    return digitalRead(indexPin);
+}