diff --git a/rotoryEncoder.ino b/rotoryEncoder.ino
index c41baec..f547322 100644
--- a/rotoryEncoder.ino
+++ b/rotoryEncoder.ino
@@ -3,10 +3,17 @@
 #define ANALOGOUT_PIN 11
 #define HOMESW_PIN 10
 
-#define NUMOFTEATH 360
-#define DEGPERSTAP 3600/(NUMOFTEATH*2)
+#define DEBUG
+#define DEBUG_CW
+#define DEBUG_A
 
-int16_t angle = 0;
+#define NUM_OF_TEATH 360*2                  // number of teath times 2
+#define MAX_RPS 1                           // max rotation per second
+#define DEG_STAP 3600/NUM_OF_TEATH          // degrees per step time 10
+#define MIN_STAP_TIME MAX_RPS/NUM_OF_TEATH  // minimum time between staps in ms
+
+uint32_t lastStap = 0; // last time of a stap
+int16_t angle = 1800;     // curent angle times 10
 
 /** setPWM
  * set the pwm dutycyle
@@ -18,13 +25,30 @@ void setPWM(uint8_t value){
 }
 
 void sens0(){
-  if(digitalRead(SENS0_PIN) != digitalRead(SENS1_PIN)){
-    angle += DEGPERSTAP * 10;
+  if(digitalRead(SENS0_PIN) != digitalRead(SENS1_PIN)
+     && millis() > lastStap + MIN_STAP_TIME){
+    angle += DEG_STAP;
+    lastStap = millis();
+    #ifdef DEBUG_CW
+    Serial.print("\t+");
+    Serial.println(angle);
+//  }else{
+//    Serial.println("CCW2");
+    #endif
   }
+  
 }
 void sens1(){
-  if(digitalRead(SENS0_PIN) != digitalRead(SENS1_PIN)){
-    angle -= DEGPERSTAP * 10;
+  if(digitalRead(SENS0_PIN) != digitalRead(SENS1_PIN)
+     && millis() > lastStap + MIN_STAP_TIME){
+    angle -= DEG_STAP;
+    lastStap = millis();
+    #ifdef DEBUG_CW
+    Serial.print("\t-");
+    Serial.println(angle);
+//  }else{
+//    Serial.println("CW2");
+    #endif
   }
 }
 
@@ -34,7 +58,7 @@ void setup(){
   // setup analog out
   pinMode(ANALOGOUT_PIN, OUTPUT);
   TCCR2A = _BV(COM2A1) | _BV(WGM20);
-  TCCR2B = _BV(CS22);
+  TCCR2B = _BV(CS20);
   setPWM(0);
 
   // setup rotory sensor
@@ -42,11 +66,19 @@ void setup(){
   pinMode(SENS1_PIN, INPUT);
   attachInterrupt(digitalPinToInterrupt(SENS0_PIN), sens0, CHANGE);
   attachInterrupt(digitalPinToInterrupt(SENS1_PIN), sens1, CHANGE);
+  
+  #ifdef DEBUG
+  Serial.begin(115200);
+
+  Serial.println("rotoryEncoder by MReenen");
+  Serial.print("deg per step: ");
+  Serial.println(DEG_STAP, DEC);
+  #endif
 }
 
 void loop(){
   // delay while checking the home sw
-  for(uint8_t i = 200; i != 0; i--){
+  for(uint16_t i = 65000; i != 0; i--){
     if(digitalRead(HOMESW_PIN) == LOW){
       angle = 0;
     }
@@ -58,7 +90,11 @@ void loop(){
   else if(angle < 0)
     angle += 3600;
 
+  #ifdef DEBUG_A
+  Serial.println(angle);
+  #endif
+
   // update PWM
   uint16_t a = angle;
-  setPWM(map(a, 0, 360, 0, 255));
+  setPWM(map(a, 0, 3600, 0, 255));
 }