From fb36da5bce185805bbc98993bcf8091a5590d521 Mon Sep 17 00:00:00 2001
From: Hendrik Langer <hendrik+dev@xd0.de>
Date: Fri, 2 Feb 2018 14:47:45 +0100
Subject: [PATCH] optimize encoder

---
 src/rotary.cpp | 51 ++++++++++++++++++++++++++------------------------
 1 file changed, 27 insertions(+), 24 deletions(-)

diff --git a/src/rotary.cpp b/src/rotary.cpp
index 21797ab..3c98ac7 100644
--- a/src/rotary.cpp
+++ b/src/rotary.cpp
@@ -5,6 +5,8 @@
 
 #include "rotary.h"
 
+#include "freertos/task.h"
+
 //using namespace std;
 
 Rotary* Rotary::instance = NULL;
@@ -43,16 +45,33 @@ void Rotary::task(void *pvParameters) {
   xTaskToNotify = xTaskGetCurrentTaskHandle();
   uint32_t ulNotificationValue;
   while(true) {
-    ulNotificationValue = ulTaskNotifyTake( pdFALSE, portMAX_DELAY );
+    ulNotificationValue = ulTaskNotifyTake( pdTRUE, portMAX_DELAY );
+    delay(2);  // wait until bounce settled
+
+    static uint8_t old_AB = 0;
+    // grey code
+    // http://hades.mech.northwestern.edu/index.php/Rotary_Encoder
+    // also read up on 'Understanding Quadrature Encoded Signals'
+    // https://www.pjrc.com/teensy/td_libs_Encoder.html
+    // another interesting lib: https://github.com/0xPIT/encoder/blob/arduino/ClickEncoder.cpp
+    static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
+
+    old_AB <<= 2;
+    old_AB |= ((digitalRead(pinB))?(1<<1):0) | ((digitalRead(pinA))?(1<<0):0);
+    encoderPos += ( enc_states[( old_AB & 0x0f )]);
+
+    ulNotificationValue = ulTaskNotifyTake( pdTRUE, 0 ); // clear pending notifications
+
     debouncePulses++;
-    if (debouncePulses > 3) {
-    debouncePulses = 0;
-    if (encoderPos > encoderPosOld+1) value++;
-    else if (encoderPos < encoderPosOld-1) value--;
-    else continue;
-    encoderPosOld = encoderPos;
-    if (callback) callback(value);
+    if (debouncePulses > 3) { // update every 4 pulses
+      debouncePulses = 0;
+      if (encoderPos > encoderPosOld+1) value++;  // if the value has at least changed for 2
+      else if (encoderPos < encoderPosOld-1) value--;
+      else continue;   // otherwise skip
+      encoderPosOld = encoderPos;
+      if (callback) callback(value);
     }
+
   }
   vTaskDelete(NULL);
 }
@@ -64,25 +83,9 @@ bool Rotary::registerCallback(std::function<void(int)> callback) {
 
 // Interrupt on changing state
 void Rotary::doEncoder() {
-  if ((millis() - instance->debounceMillis) > instance->debounceDelay) {
-    instance->debounceMillis = millis();
-
     BaseType_t xHigherPriorityTaskWoken = pdFALSE;
 
-    static uint8_t old_AB = 0;
-    // grey code
-    // http://hades.mech.northwestern.edu/index.php/Rotary_Encoder
-    // also read up on 'Understanding Quadrature Encoded Signals'
-    // https://www.pjrc.com/teensy/td_libs_Encoder.html
-    // another interesting lib: https://github.com/0xPIT/encoder/blob/arduino/ClickEncoder.cpp
-    static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
-
-    old_AB <<= 2;
-    old_AB |= ((digitalRead(instance->pinB))?(1<<1):0) | ((digitalRead(instance->pinA))?(1<<0):0);
-    instance->encoderPos += ( enc_states[( old_AB & 0x0f )]);
-
     configASSERT( instance->xTaskToNotify != NULL );
     vTaskNotifyGiveFromISR( instance->xTaskToNotify, &xHigherPriorityTaskWoken );
     portYIELD_FROM_ISR(); //portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
-  }
 }