simple QEI library. no return speed, only return angle.
Dependents: WRS_mechanamu_test
Revision 2:62821df48957, committed 2020-03-02
- Comitter:
- sgrsn
- Date:
- Mon Mar 02 06:23:11 2020 +0000
- Parent:
- 0:bffc97496048
- Commit message:
- Add getSpeed()
Changed in this revision
QEI.cpp | Show annotated file Show diff for this revision Revisions of this file |
QEI.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r bffc97496048 -r 62821df48957 QEI.cpp --- a/QEI.cpp Mon Aug 20 04:54:24 2018 +0000 +++ b/QEI.cpp Mon Mar 02 06:23:11 2020 +0000 @@ -2,21 +2,77 @@ 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) +QEI::QEI(PinName A, PinName B, int ppr) : channelA(A), channelB(B), channelZ(NC) +{ + Timer tmp; + _timer = &tmp; + _ppr = ppr; + init(); +} + +QEI::QEI(PinName A, PinName B, PinName Z, int ppr) : channelA(A), channelB(B), channelZ(Z) +{ + Timer tmp; + _timer = &tmp; + _ppr = ppr; + init(); + channelZ.rise(this, &QEI::encodeZ); + channelZ.fall(this, &QEI::encodeZ); +} + +QEI::QEI(PinName A, PinName B, int ppr, Timer *timer) : channelA(A), channelB(B), channelZ(NC) +{ + _timer = timer; + _ppr = ppr; + init(); +} + +QEI::QEI(PinName A, PinName B, PinName Z, int ppr, Timer *timer) : channelA(A), channelB(B), channelZ(Z) +{ + _timer = timer; + _ppr = ppr; + init(); + channelZ.rise(this, &QEI::encodeZ); + channelZ.fall(this, &QEI::encodeZ); +} + +void QEI::init() { 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; + _IsInterrupt = false; + + _timer -> start(); +} + +float QEI::getDegree() +{ + return float(position) * 360.0/(_ppr*4.0); } -float QEI::getAngle() +float QEI::getSpeed() { - return float(position) * 360.0/(_ppr*4.0); + static float last_time = 0; + static float last_degree = 0; + float current_time =_timer -> read(); + float dt = current_time - last_time; + float current_degree = float(position) * 360.0/(_ppr*4.0); + float speed = (current_degree - last_degree) / dt; + last_degree = current_degree; + last_time = current_time; + return speed; +} + +bool QEI::IsInterruptZ() +{ + bool tmp = _IsInterrupt; + _IsInterrupt = false; + return tmp; } void QEI::encode(void) @@ -29,4 +85,9 @@ position += encodeTable[currState | (prevState<<2)]; prevState = currState; } +} + +void QEI::encodeZ() +{ + _IsInterrupt = true; } \ No newline at end of file
diff -r bffc97496048 -r 62821df48957 QEI.h --- a/QEI.h Mon Aug 20 04:54:24 2018 +0000 +++ b/QEI.h Mon Mar 02 06:23:11 2020 +0000 @@ -3,33 +3,37 @@ #include "mbed.h" - -#define PREV_MASK 0x1 //Mask for the previous state in determining direction -//of rotation. -#define CURR_MASK 0x2 //Mask for the current state in determining direction -//of rotation. -#define INVALID 0x3 //XORing two states where both bits have changed. - - class QEI { public: QEI(PinName A, PinName B, int ppr); + QEI(PinName A, PinName B, PinName Z, int ppr); + QEI(PinName A, PinName B, int ppr, Timer *timer); + QEI(PinName A, PinName B, PinName Z, int ppr, Timer *timer); - float getAngle(); + float getDegree(); + float getSpeed(); + bool IsInterruptZ(); private: + void init(); void encode(void); - + void encodeZ(); + InterruptIn channelA; InterruptIn channelB; + InterruptIn channelZ; + Timer *_timer; + float angle; int currState; int prevState; int position; float _ppr; + + bool _IsInterrupt; }; #endif