simple QEI library. no return speed, only return angle.
Dependents: WRS_mechanamu_test
Diff: QEI.cpp
- Revision:
- 0:bffc97496048
- Child:
- 1:6fa863d09d45
- Child:
- 2:62821df48957
diff -r 000000000000 -r bffc97496048 QEI.cpp --- /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