Diff: QEI.cpp

Revision:

0:bffc97496048

Child:

1:6fa863d09d45

Child:

2:62821df48957

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.cpp	Mon Aug 20 04:54:24 2018 +0000
@@ -0,0 +1,32 @@
+#include "QEI.h"
+
+const int8_t encodeTable[] = {0, -1,  1,  0, 1,  0,  0, -1, -1,  0,  0,  1, 0,  1, -1,  0 };
+
+QEI::QEI(PinName A, PinName B, int ppr) : channelA(A), channelB(B)
+{
+    channelA.rise(this, &QEI::encode);
+    channelB.rise(this, &QEI::encode);
+    channelA.fall(this, &QEI::encode);
+    channelB.fall(this, &QEI::encode);
+    _ppr = ppr;
+    currState = 0;
+    prevState = 0;
+    position = 0;
+}
+
+float QEI::getAngle()
+{
+    return float(position) * 360.0/(_ppr*4.0);
+}
+
+void QEI::encode(void)
+{
+    int8_t chanA  = channelA.read();
+    int8_t chanB  = channelB.read();
+    currState = chanA | (chanB << 1);
+    
+    if (prevState != currState) {
+        position += encodeTable[currState | (prevState<<2)];
+        prevState = currState;
+    }
+}
\ No newline at end of file