Part 4

src/rotary_encoder_driver.cpp

@@ -8,9 +8,15 @@ RotaryEncoderDriver::RotaryEncoderDriver()
     , count(0)
     , prev_count(0)
     , speed_ready(false)
+    , hist_idx(0)
     , led(5)
     , eA(2)
-    , eB(5) {}
+    , eB(5)
+{
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = 0;
+    }
+}
 
 
 void RotaryEncoderDriver::begin() {
@@ -23,7 +29,7 @@ void RotaryEncoderDriver::begin() {
     EICRA |= (1<<ISC01) | (1<<ISC00);
     EIMSK |= (1<<INT0);
 
-    timer.init(5000);
+    timer.init(4000);
 }
 
 void RotaryEncoderDriver::update() {
@@ -47,13 +53,17 @@ int RotaryEncoderDriver::position() {
 } 
 
 void RotaryEncoderDriver::update_delta() {
-    
-        delta = count - prev_count;
 
-        prev_count = count;
+    int16_t oldest = history[hist_idx];
+    history[hist_idx] = count;
+    hist_idx = (hist_idx + 1) % HIST_LEN;
+
+    delta = count - oldest;   // breyting yfir HIST_LEN * Ts (40 ms) glugga
+
+    prev_count = count;
+
+    speed_ready = true;
 
-        speed_ready = true;
-    
     led.set_lo();
 }
 
@@ -67,7 +77,7 @@ float RotaryEncoderDriver::count_per_sec() {
 
     SREG = oldSREG;
 
-    return local_delta / 0.005f;
+    return local_delta / (HIST_LEN * 0.004f);
 }
 
 
@@ -108,6 +118,11 @@ void RotaryEncoderDriver::reset_speed_measurement()
     delta = 0;
     speed_ready = false;
 
+    for (uint8_t i = 0; i < HIST_LEN; i++) {
+        history[i] = count;
+    }
+    hist_idx = 0;
+
     SREG = oldSREG;
 }