Diff: PolledQEI.cpp

Revision:

0:7c16b1d2f01e

Child:

1:72e6e124a7e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PolledQEI.cpp	Tue Aug 30 19:52:34 2011 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "PolledQEI.h"
+
+
+
+
+void PolledQEI::setSampleFrequency(int i = 10000) {
+    _sample_uSec = i;
+    _actEncPos = 0;
+    _ticker->attach_us( this, &PolledQEI::check, _sample_uSec );
+}
+
+int PolledQEI::pos(void) {
+    return _actEncPos;
+}
+
+int PolledQEI::dir(void) {
+    return _dir;
+}
+
+int PolledQEI::rev(void) {
+    return _actEncRev;
+}
+
+void PolledQEI::countRev(void) {
+    if (_dir) {
+        _actEncRev++;
+    } else {
+        _actEncRev--;
+    }
+}
+
+void PolledQEI::init(PinName a, PinName b, PinName z = NC) {
+
+    _phaseA = new DigitalIn( a );
+    _phaseA->mode( PullUp );
+    _phaseB = new DigitalIn( b );
+    _phaseB->mode( PullUp );
+    if (z != NC) {
+        _phaseZ = new InterruptIn( z );
+        _phaseZ->mode( PullUp );
+        _phaseZ->fall(this, &PolledQEI::countRev);
+    }
+    _actEncRev = 0;
+    _actEncPos = 0;
+    _dir = 0;
+
+    _ticker = new Ticker;
+}
+
+void PolledQEI::check (void) {
+    unsigned int pA = _phaseA->read();
+    unsigned int pB = _phaseB->read();
+    unsigned int newV = 0, diffV = 0;
+
+
+    // convert gray to binary
+    if ( pA ) {
+        newV = 3;
+    }
+    if ( pB ) {
+        newV ^= 1;
+    }
+
+    diffV = last - newV;			    // difference last - new
+    if ( diffV & 1 ) {				    // bit 0 = value (1)
+        last = newV;				    // store new as next last
+        _actEncPos -= (diffV & 2) - 1;  // bit 1 = direction (+/-)
+        _dir = (diffV & 2)?CW:CCW;
+    }
+}
+
+