simple QEI library. no return speed, only return angle.
Dependents: WRS_mechanamu_test
QEI.cpp
- Committer:
- sgrsn
- Date:
- 2018-11-14
- Revision:
- 1:6fa863d09d45
- Parent:
- 0:bffc97496048
File content as of revision 1:6fa863d09d45:
#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, Timer *t) : channelA(A), channelB(B) { timer = t; 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; last_position = 0; time = timer->read(); } float QEI::getAngle() { return float(position) * 360.0/(_ppr*4.0); } float QEI::getSpeed() { float dt = timer->read() - time; float dangle = float(position - last_position) * 360.0/(_ppr*4.0); float speed = dangle / dt; time = timer->read(); last_position = position; return speed; } float QEI::getDisAngle() { float dangle = float(position - last_position) * 360.0/(_ppr*4.0); last_position = position; return dangle; } void QEI::reset() { position = 0; last_position = 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; } }