diff --git a/src/main.cpp b/src/main.cpp
index 4429afc..5a6e145 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,6 +9,19 @@ static uint8_t led_duration = 4;
  */
 void configureTimer()
 {
+    // Configure Timer 0 to generate periodic interrupts
+    TCCR0A = 0;// set entire TCCR0A register to 0
+    TCCR0B = 0;// same for TCCR0B
+    TCNT0  = 0;//initialize counter value to 0
+    // set compare match register for 2khz increments
+    OCR0A = 124;// = (16*10^6) / (2000*64) - 1 (must be <256)
+    // turn on CTC mode
+    TCCR0A |= (1 << WGM01);
+    // Set CS01 and CS00 bits for 64 prescaler
+    TCCR0B |= (1 << CS01) | (1 << CS00);   
+    // enable timer compare interrupt
+    TIMSK0 |= (1 << OCIE0A);
+
     // Configure Timer 1 for PWM @ 25 kHz.
     TCCR1A = 0;   // undo the configuration done by...
     TCCR1B = 0;   // ...the Arduino core library...
@@ -64,64 +77,66 @@ void setup()
     // configure timer
     Serial.println("configure timer for 25kHz PWM...");
     configureTimer();
-
     Serial.println("fan control setup done.");
+    Serial.println("Enable interrupts...");
+    sei();
+}
+
+static uint8_t tacho_last = 0;
+static uint8_t tacho_counter = 0;
+static uint32_t iterator = 0;
+static uint32_t period = 0;
+static uint16_t rotations = 0;
+
+static uint16_t led_a_period = (uint16_t)-1;
+static uint16_t led_a_counter = led_a_period;
+
+static uint16_t led_b_period = (uint16_t)-1;
+static uint16_t led_b_counter = led_b_period;
+
+ISR(TIMER0_COMPA_vect){
+    iterator++;
+
+    uint8_t tacho_current = digitalRead(7);
+
+    /* Get number of loop iterations per fan revolution */
+    if (!tacho_last && tacho_current) {
+        if (tacho_counter == 0) {
+            tacho_counter = 1;
+        } else {
+            tacho_counter = 0;
+            period = iterator;
+            led_a_period = period / 3;
+            led_b_period = period * 20 / 61;
+            iterator = 0;
+            rotations++;
+        }
+    }
+    tacho_last = tacho_current;
+
+    /* Emit strobe for LED A */
+    led_a_counter--;
+    if (led_a_counter < led_duration) {
+        digitalWrite(2, HIGH);
+        if (!led_a_counter) {
+            led_a_counter = led_a_period;
+        }
+    } else {
+        digitalWrite(2, LOW);
+    }
+
+    /* Emit strobe for LED B */
+    led_b_counter--;
+    if (led_b_counter < led_duration) {
+        digitalWrite(3, HIGH);
+        if (!led_b_counter) {
+            led_b_counter = led_b_period;
+        }
+    } else {
+        digitalWrite(3, LOW);
+    }
 }
 
 void loop()
 {
-    uint8_t tacho_last = 0;
-    uint8_t tacho_counter = 0;
-    uint32_t iterator = 0;
-    uint32_t period = 0;
-    uint16_t rotations = 0;
-
-    uint16_t led_a_period = (uint16_t)-1;
-    uint16_t led_a_counter = led_a_period;
-
-    uint16_t led_b_period = (uint16_t)-1;
-    uint16_t led_b_counter = led_b_period;
-
-    while (1) {
-        iterator++;
-
-        uint8_t tacho_current = digitalRead(7);
-
-        /* Get number of loop iterations per fan revolution */
-        if (!tacho_last && tacho_current) {
-            if (tacho_counter == 0) {
-                tacho_counter = 1;
-            } else {
-                tacho_counter = 0;
-                period = iterator;
-                led_a_period = period / 3;
-                led_b_period = period * 20 / 61;
-                iterator = 0;
-                rotations++;
-            }
-        }
-        tacho_last = tacho_current;
-
-        /* Emit strobe for LED A */
-        led_a_counter--;
-        if (led_a_counter < led_duration) {
-            digitalWrite(2, HIGH);
-            if (!led_a_counter) {
-                led_a_counter = led_a_period;
-            }
-        } else {
-            digitalWrite(2, LOW);
-        }
-
-        /* Emit strobe for LED B */
-        led_b_counter--;
-        if (led_b_counter < led_duration) {
-            digitalWrite(3, HIGH);
-            if (!led_b_counter) {
-                led_b_counter = led_b_period;
-            }
-        } else {
-            digitalWrite(3, LOW);
-        }
-    }  
-}
+}
\ No newline at end of file