diff --git a/src/main.cpp b/src/main.cpp
index 2b60d34..199b335 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -14,10 +14,10 @@ void configureTimer()
     TCNT1  = 0;   // reset timer
     // non-inverted PWM on ch. A, B and C, mode 10: ph. correct PWM, TOP = ICR1
     // TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(COM1C1) | _BV(WGM11);
+
+    ICR1   = 640;       // TOP = 320, @16Mhz CPU -> 25kHz PWM
+    OCR1A = 500;          // Set duty-cycle
     TCCR1B = _BV(WGM13) | _BV(CS10); // prescaler = 1
-    TCCR1C = _BV(WGM13) | _BV(CS10); // on PIN 9,10,11
-    ICR1   = 320;       // TOP = 320, @16Mhz CPU -> 25kHz PWM
-    OCR1A = 5;          // Set duty-cycle
     // Configure Timer 3
     // TCCR3A = 0;
     // TCNT3  = 0;
@@ -67,19 +67,34 @@ void setup()
 
 void loop()
 {
-    int last = digitalRead(7);
+    uint8_t tacho_last = 0;
+    uint8_t tacho_counter = 0;
+    uint32_t iterator = 0;
+    uint32_t period = 0;
+    uint16_t rotations = 0;
 
     while (1) {
-        int next = digitalRead(7);
+        iterator++;
 
-        if (!last && next) {
-            digitalWrite(2, HIGH);
+        uint8_t tacho_current = digitalRead(7);
+
+        if (!tacho_last && tacho_current) {
+            if (tacho_counter == 0) {
+                tacho_counter = 1;
+                digitalWrite(2, LOW);
+            } else {
+                tacho_counter = 0;
+                digitalWrite(2, HIGH);
+                period = iterator;
+                iterator = 0;
+                rotations++;
+            }
         } else {
             digitalWrite(2, LOW);
         }
 
-        last = next;
+        tacho_last = tacho_current;
 
-        delayMicroseconds(200);
+        delayMicroseconds(100);
     }  
 }