Juergen Gnoss
/
PolledQEI
A polled Quadrature encoder Class
PolledQEI.cpp@1:72e6e124a7e6, 2011-08-30 (annotated)
- Committer:
- jgnoss
- Date:
- Tue Aug 30 20:03:36 2011 +0000
- Revision:
- 1:72e6e124a7e6
- Parent:
- 0:7c16b1d2f01e
Added Phase Z and Rev Counter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jgnoss | 1:72e6e124a7e6 | 1 | #include "mbed.h" |
jgnoss | 1:72e6e124a7e6 | 2 | #include "PolledQEI.h" |
jgnoss | 1:72e6e124a7e6 | 3 | |
jgnoss | 1:72e6e124a7e6 | 4 | |
jgnoss | 1:72e6e124a7e6 | 5 | |
jgnoss | 1:72e6e124a7e6 | 6 | |
jgnoss | 1:72e6e124a7e6 | 7 | void PolledQEI::setSampleFrequency(int i = 10000) { |
jgnoss | 1:72e6e124a7e6 | 8 | _sample_uSec = i; |
jgnoss | 1:72e6e124a7e6 | 9 | _actEncPos = 0; |
jgnoss | 1:72e6e124a7e6 | 10 | _ticker->attach_us( this, &PolledQEI::check, _sample_uSec ); |
jgnoss | 1:72e6e124a7e6 | 11 | } |
jgnoss | 1:72e6e124a7e6 | 12 | |
jgnoss | 1:72e6e124a7e6 | 13 | int PolledQEI::pos(void) { |
jgnoss | 1:72e6e124a7e6 | 14 | return _actEncPos; |
jgnoss | 1:72e6e124a7e6 | 15 | } |
jgnoss | 1:72e6e124a7e6 | 16 | |
jgnoss | 1:72e6e124a7e6 | 17 | int PolledQEI::dir(void) { |
jgnoss | 1:72e6e124a7e6 | 18 | return _dir; |
jgnoss | 1:72e6e124a7e6 | 19 | } |
jgnoss | 1:72e6e124a7e6 | 20 | |
jgnoss | 1:72e6e124a7e6 | 21 | int PolledQEI::rev(void) { |
jgnoss | 1:72e6e124a7e6 | 22 | return _actEncRev; |
jgnoss | 1:72e6e124a7e6 | 23 | } |
jgnoss | 1:72e6e124a7e6 | 24 | |
jgnoss | 1:72e6e124a7e6 | 25 | void PolledQEI::countRev(void) { |
jgnoss | 1:72e6e124a7e6 | 26 | if (_dir) { |
jgnoss | 1:72e6e124a7e6 | 27 | _actEncRev++; |
jgnoss | 1:72e6e124a7e6 | 28 | } else { |
jgnoss | 1:72e6e124a7e6 | 29 | _actEncRev--; |
jgnoss | 1:72e6e124a7e6 | 30 | } |
jgnoss | 1:72e6e124a7e6 | 31 | } |
jgnoss | 1:72e6e124a7e6 | 32 | |
jgnoss | 1:72e6e124a7e6 | 33 | void PolledQEI::init(PinName a, PinName b, PinName z = NC) { |
jgnoss | 1:72e6e124a7e6 | 34 | |
jgnoss | 1:72e6e124a7e6 | 35 | _phaseA = new DigitalIn( a ); |
jgnoss | 1:72e6e124a7e6 | 36 | _phaseA->mode( PullUp ); |
jgnoss | 1:72e6e124a7e6 | 37 | _phaseB = new DigitalIn( b ); |
jgnoss | 1:72e6e124a7e6 | 38 | _phaseB->mode( PullUp ); |
jgnoss | 1:72e6e124a7e6 | 39 | if (z != NC) { |
jgnoss | 1:72e6e124a7e6 | 40 | _phaseZ = new InterruptIn( z ); |
jgnoss | 1:72e6e124a7e6 | 41 | _phaseZ->mode( PullUp ); |
jgnoss | 1:72e6e124a7e6 | 42 | _phaseZ->fall(this, &PolledQEI::countRev); |
jgnoss | 1:72e6e124a7e6 | 43 | } |
jgnoss | 1:72e6e124a7e6 | 44 | _actEncRev = 0; |
jgnoss | 1:72e6e124a7e6 | 45 | _actEncPos = 0; |
jgnoss | 1:72e6e124a7e6 | 46 | _dir = 0; |
jgnoss | 1:72e6e124a7e6 | 47 | |
jgnoss | 1:72e6e124a7e6 | 48 | _ticker = new Ticker; |
jgnoss | 1:72e6e124a7e6 | 49 | } |
jgnoss | 1:72e6e124a7e6 | 50 | |
jgnoss | 1:72e6e124a7e6 | 51 | void PolledQEI::check (void) { |
jgnoss | 1:72e6e124a7e6 | 52 | unsigned int pA = _phaseA->read(); |
jgnoss | 1:72e6e124a7e6 | 53 | unsigned int pB = _phaseB->read(); |
jgnoss | 1:72e6e124a7e6 | 54 | unsigned int newV = 0, diffV = 0; |
jgnoss | 1:72e6e124a7e6 | 55 | |
jgnoss | 1:72e6e124a7e6 | 56 | |
jgnoss | 1:72e6e124a7e6 | 57 | // convert gray to binary |
jgnoss | 1:72e6e124a7e6 | 58 | if ( pA ) { |
jgnoss | 1:72e6e124a7e6 | 59 | newV = 3; |
jgnoss | 1:72e6e124a7e6 | 60 | } |
jgnoss | 1:72e6e124a7e6 | 61 | if ( pB ) { |
jgnoss | 1:72e6e124a7e6 | 62 | newV ^= 1; |
jgnoss | 1:72e6e124a7e6 | 63 | } |
jgnoss | 1:72e6e124a7e6 | 64 | |
jgnoss | 1:72e6e124a7e6 | 65 | diffV = last - newV; // difference last - new |
jgnoss | 1:72e6e124a7e6 | 66 | if ( diffV & 1 ) { // bit 0 = value (1) |
jgnoss | 1:72e6e124a7e6 | 67 | last = newV; // store new as next last |
jgnoss | 1:72e6e124a7e6 | 68 | _actEncPos -= (diffV & 2) - 1; // bit 1 = direction (+/-) |
jgnoss | 1:72e6e124a7e6 | 69 | _dir = (diffV & 2)?CW:CCW; |
jgnoss | 1:72e6e124a7e6 | 70 | } |
jgnoss | 1:72e6e124a7e6 | 71 | } |
jgnoss | 1:72e6e124a7e6 | 72 | |
jgnoss | 1:72e6e124a7e6 | 73 |