diff --git a/include/motor_controller.h b/include/motor_controller.h
index c085214..21543f5 100644
--- a/include/motor_controller.h
+++ b/include/motor_controller.h
@@ -4,6 +4,7 @@
 #include "rotary_encoder_driver.h"
 #include "motor_driver.h"
 #include "p_controller.h"
+#include "digital_out.h"
 
 class State;
 
@@ -11,15 +12,21 @@ class State;
 class MotorController
 {
 private:
+    static constexpr double DEFAULT_REFERENCE_RPM = 40.0;
+    static constexpr double PWM_PER_PERCENT = 255.0 / 100.0;
+    static const uint8_t LED_PIN = 5;   // PB5 = D13 = LED_BUILTIN
+
     State* current_state;
 
-    State* initialization_state;
-    State* operational_state;
+    State& initialization_state;
+    State& operational_state;
 
     RotaryEncoderDriver& encoder;
     MotorDriver& motor;
     P_controller& controller;
 
+    Digital_out led;
+
     double reference_rpm;
 
 
@@ -27,13 +34,9 @@ public:
     MotorController(
         RotaryEncoderDriver& encoder,
         MotorDriver& motor,
-        P_controller& controller
-    );
-
-
-    void set_states(
-        State* initialization,
-        State* operational
+        P_controller& controller,
+        State& initialization,
+        State& operational
     );
 
 
@@ -43,7 +46,7 @@ public:
 
     void command(char c);
 
-    void transition_to(State* new_state);
+    void transition_to(State& new_state);
 
 
     void to_initialization();
@@ -56,6 +59,10 @@ public:
     void stop_motor();
 
     void reset_speed_measurement();
+
+    void led_on();
+
+    void led_off();
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/rotary_encoder_driver.h b/include/rotary_encoder_driver.h
index d097eba..ef1ed44 100644
--- a/include/rotary_encoder_driver.h
+++ b/include/rotary_encoder_driver.h
@@ -4,37 +4,36 @@
 #include <stdint.h>
 
 #include "digital_in.h"
-//#include "digital_out.h"
 #include "timer_usec.h"
 
 class RotaryEncoderDriver {
-    static const uint8_t HIST_LEN = 4;   // 4 x 4 ms = 16 ms gluggi fyrir rpm-utreikning
+public:
+    static constexpr uint16_t SAMPLE_PERIOD_US = 4000;   // Ts = 4 ms
+    static constexpr float COUNTS_PER_REV = 1050.0f;
 
-    volatile uint8_t cycle;
-    volatile int16_t count;
-    volatile int16_t prev_count;
+private:
+    static const uint8_t HIST_LEN = 4;   // 4 x Ts = 16 ms gluggi fyrir rpm-utreikning
+
+    volatile int32_t count;
+    volatile int16_t delta;
     volatile bool speed_ready;
 
-    volatile int16_t history[HIST_LEN];
+    volatile int32_t history[HIST_LEN];
     volatile uint8_t hist_idx;
 
-    //Digital_out led;
-
     Digital_in eA;
     Digital_in eB;
 
     Timer_usec timer;
 
 public:
-    volatile int16_t delta;
-
     RotaryEncoderDriver();
 
     void begin();
 
     void update();
 
-    int position(); 
+    int32_t position();
 
     void update_delta();
 
diff --git a/include/state.h b/include/state.h
index 4f58648..2707215 100644
--- a/include/state.h
+++ b/include/state.h
@@ -6,7 +6,7 @@ class MotorController;
 class State
 {
 protected:
-    MotorController* context;
+    MotorController* context = nullptr;
 
 public:
     virtual ~State()
diff --git a/src/initialization_state.cpp b/src/initialization_state.cpp
index f7db3bb..5d27d09 100644
--- a/src/initialization_state.cpp
+++ b/src/initialization_state.cpp
@@ -9,7 +9,7 @@ void InitializationState::enter()
     Serial.println();
     Serial.println("State: Initialization");
 
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 
     context->stop_motor();
 
diff --git a/src/main.cpp b/src/main.cpp
index 6f59596..08c2f2d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -15,14 +15,9 @@ RotaryEncoderDriver encoder;
 
 MotorDriver motor;
 
-P_controller controller(12.5);
+const double KP = 12.5;
 
-
-MotorController motor_controller(
-    encoder,
-    motor,
-    controller
-);
+P_controller controller(KP);
 
 
 InitializationState initialization_state;
@@ -30,6 +25,16 @@ InitializationState initialization_state;
 OperationalState operational_state;
 
 
+// Stöðurnar verða að vera skilgreindar á undan motor_controller
+MotorController motor_controller(
+    encoder,
+    motor,
+    controller,
+    initialization_state,
+    operational_state
+);
+
+
 int main()
 {
     init();
@@ -37,11 +42,6 @@ int main()
     Serial.begin(115200);
 
 
-    pinMode(LED_BUILTIN, OUTPUT);
-
-    digitalWrite(LED_BUILTIN, LOW);
-
-
     cli();
 
     encoder.begin();
@@ -51,12 +51,6 @@ int main()
     sei();
 
 
-    motor_controller.set_states(
-        &initialization_state,
-        &operational_state
-    );
-
-
     motor_controller.begin();
 
 
diff --git a/src/motor_controller.cpp b/src/motor_controller.cpp
index 23cd265..feb5d88 100644
--- a/src/motor_controller.cpp
+++ b/src/motor_controller.cpp
@@ -7,45 +7,41 @@
 MotorController::MotorController(
     RotaryEncoderDriver& encoder,
     MotorDriver& motor,
-    P_controller& controller
+    P_controller& controller,
+    State& initialization,
+    State& operational
 )
     : current_state(nullptr),
-      initialization_state(nullptr),
-      operational_state(nullptr),
+      initialization_state(initialization),
+      operational_state(operational),
       encoder(encoder),
       motor(motor),
       controller(controller),
-      reference_rpm(40.0)
+      led(LED_PIN),
+      reference_rpm(DEFAULT_REFERENCE_RPM)
 {
-}
-
-
-void MotorController::set_states(
-    State* initialization,
-    State* operational
-)
-{
-    initialization_state = initialization;
-    operational_state = operational;
+    initialization_state.set_context(this);
+    operational_state.set_context(this);
 }
 
 
 void MotorController::begin()
 {
+    led.init();
+    led.set_lo();
+
     transition_to(initialization_state);
 }
 
 
-void MotorController::transition_to(State* new_state)
+void MotorController::transition_to(State& new_state)
 {
     if (current_state != nullptr)
     {
         current_state->exit();
     }
 
-    current_state = new_state;
-
-    current_state->set_context(this);
+    current_state = &new_state;
 
     current_state->enter();
 }
@@ -91,7 +87,7 @@ void MotorController::run_speed_controller()
             controller.update(reference_rpm, actual);
 
         double pwm =
-            pwm_percent * 2.55;
+            pwm_percent * PWM_PER_PERCENT;
 
         motor.set(pwm);
     }
@@ -107,4 +103,16 @@ void MotorController::stop_motor()
 void MotorController::reset_speed_measurement()
 {
     encoder.reset_speed_measurement();
-}
\ No newline at end of file
+}
+
+
+void MotorController::led_on()
+{
+    led.set_hi();
+}
+
+
+void MotorController::led_off()
+{
+    led.set_lo();
+}
diff --git a/src/operational_state.cpp b/src/operational_state.cpp
index e052095..30d0116 100644
--- a/src/operational_state.cpp
+++ b/src/operational_state.cpp
@@ -8,7 +8,7 @@ void OperationalState::enter()
 {
     Serial.println("State: Operational");
 
-    digitalWrite(LED_BUILTIN, HIGH);
+    context->led_on();
 }
 
 
@@ -33,5 +33,5 @@ void OperationalState::exit()
 {
     context->stop_motor();
 
-    digitalWrite(LED_BUILTIN, LOW);
+    context->led_off();
 }
\ No newline at end of file
diff --git a/src/rotary_encoder_driver.cpp b/src/rotary_encoder_driver.cpp
index 2faf856..2c75a4b 100644
--- a/src/rotary_encoder_driver.cpp
+++ b/src/rotary_encoder_driver.cpp
@@ -1,80 +1,70 @@
 #include "rotary_encoder_driver.h"
 #include <avr/interrupt.h>
 
-RotaryEncoderDriver* driver = nullptr;
+static RotaryEncoderDriver* driver = nullptr;
 
 RotaryEncoderDriver::RotaryEncoderDriver()
-    : cycle(0)
-    , count(0)
-    , prev_count(0)
+    : count(0)
+    , delta(0)
     , speed_ready(false)
     , hist_idx(0)
     , eA(2)
     , eB(5)
-    , delta(0)
 {
-    for (uint8_t i = 0; i < HIST_LEN; i++) 
+    for (uint8_t i = 0; i < HIST_LEN; i++)
     {
         history[i] = 0;
     }
 }
 
 
-void RotaryEncoderDriver::begin() 
+void RotaryEncoderDriver::begin()
 {
     driver = this;
 
-    //led.init();
     eA.init();
     eB.init();
 
     EICRA |= (1<<ISC01) | (1<<ISC00);
     EIMSK |= (1<<INT0);
 
-    timer.init(4000);
+    timer.init(SAMPLE_PERIOD_US);
 }
 
-void RotaryEncoderDriver::update() 
+void RotaryEncoderDriver::update()
 {
     count += eB.is_hi() ? 1 : -1;
-    //led.set_hi();
 }
 
-int RotaryEncoderDriver::position() 
+int32_t RotaryEncoderDriver::position()
 {
 
     uint8_t oldSREG = SREG;
 
     cli();
 
-    int16_t local_count = count;
+    int32_t local_count = count;
 
     SREG = oldSREG;
 
-    //led.set_lo();
-
     return local_count;
-} 
+}
 
-void RotaryEncoderDriver::update_delta() 
+void RotaryEncoderDriver::update_delta()
 {
 
-    int16_t oldest = history[hist_idx];
+    int32_t oldest = history[hist_idx];
 
     history[hist_idx] = count;
 
     hist_idx = (hist_idx + 1) % HIST_LEN;
 
-    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (16 ms) glugga
-
-    prev_count = count;
+    delta = (int16_t)(count - oldest);   // breyting yfir HIST_LEN * Ts (16 ms) glugga
 
     speed_ready = true;
-
-    //led.set_lo();
 }
 
-float RotaryEncoderDriver::count_per_sec() 
+float RotaryEncoderDriver::count_per_sec()
 {
 
     uint8_t oldSREG = SREG;
@@ -85,14 +75,12 @@ float RotaryEncoderDriver::count_per_sec()
 
     SREG = oldSREG;
 
-    return local_delta / (HIST_LEN * 0.004f);
+    return local_delta / (HIST_LEN * SAMPLE_PERIOD_US * 1e-6f);
 }
 
 
 float RotaryEncoderDriver::rpm()
 {
-    const float COUNTS_PER_REV = 1050.0f;
-
     return count_per_sec() * 60.0f / COUNTS_PER_REV;
 }
 
@@ -121,12 +109,10 @@ void RotaryEncoderDriver::reset_speed_measurement()
 
     cli();
 
-    cycle = 0;
-    prev_count = count;
     delta = 0;
     speed_ready = false;
 
-    for (uint8_t i = 0; i < HIST_LEN; i++) 
+    for (uint8_t i = 0; i < HIST_LEN; i++)
     {
         history[i] = count;
     }
@@ -136,13 +122,11 @@ void RotaryEncoderDriver::reset_speed_measurement()
     SREG = oldSREG;
 }
 
-ISR(INT0_vect) 
+ISR(INT0_vect)
 {
     driver->update();
 }
-ISR(TIMER1_COMPA_vect) 
+ISR(TIMER1_COMPA_vect)
 {
-    driver->update_delta();    
+    driver->update_delta();
 }
-
-