diff --git a/main.cpp b/main.cpp
index 54faf56..6163131 100644
--- a/main.cpp
+++ b/main.cpp
@@ -5,46 +5,61 @@
 #include "digital_in.h"
 #include "digital_out.h"
 
+class RotaryEncoderDriver {
+    int count{0};
+    bool last_lo{false};
+
+    Digital_out led{5};
+
+    Digital_in eA;
+    Digital_in eB;
+
+public:
+    RotaryEncoderDriver(unsigned short pin_a = 4, unsigned short pin_b = 5) : eA(pin_a), eB(pin_b) {
+        led.init();
+        eA.init();
+        eB.init();
+        last_lo = eA.is_lo();
+    }
+
+    int position() {
+        bool curr_hi = eA.is_hi();
+
+        if (last_lo && curr_hi) {
+            count += eB.is_hi() ? 1 : -1;
+            led.set_hi();
+        } else {
+            led.set_lo();
+        }
+
+        last_lo = !curr_hi;
+        return count;
+    } 
+
+};
 
 int main() {
     uint8_t cycle{0};
-    int8_t counter{0};
-    double ratio;
-
-    bool last_lo = false;
-
-    Digital_in encoderA{4};
-    Digital_in encoderB{5};
-    Digital_out led{5};
-
-    encoderA.init();
-    encoderB.init();
-    led.init();
 
+    RotaryEncoderDriver encoder;
     Serial.begin(9600);
 
+    int prev_pos = encoder.position();
     while (true) {
         _delay_us(100);
-        led.set_lo();
+        int curr_pos = encoder.position();
 
-        bool curr_hi = encoderA.is_hi();
+        if (cycle++ == 255) {
+            int delta = curr_pos - prev_pos;
+            float rpm = delta * 60.0f / (700.0f * 0.0256f);
 
-        if (last_lo && curr_hi) {
-            counter += encoderB.is_lo() ? -1 : 1;
-            led.set_hi();
+            Serial.print("Count: ");
+            Serial.print(delta);
+            Serial.print("\tRPM: ");
+            Serial.println(rpm);
+
+            prev_pos = curr_pos;
         }
-        ++cycle;
-
-        if (cycle == 255) {
-            ratio = counter / (0.0255 * 700);
-            counter = 0;
-            cycle = 0;
-            Serial.println(ratio);
-        }
-        
-        last_lo = !curr_hi;
     }
-
-
     return 0;
 }