hidaka sato / QEI_simple

simple QEI library. no return speed, only return angle.

Dependents:   WRS_mechanamu_test

QEI.cpp@2:62821df48957, 2020-03-02 (annotated)

Committer:
sgrsn
Date:
Mon Mar 02 06:23:11 2020 +0000
Revision:
2:62821df48957
Parent:
0:bffc97496048 
Add getSpeed()

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:bffc97496048 1 #include "QEI.h"
sgrsn 0:bffc97496048 2
sgrsn 0:bffc97496048 3 const int8_t encodeTable[] = {0, -1, 1, 0, 1, 0, 0, -1, -1, 0, 0, 1, 0, 1, -1, 0 };
sgrsn 0:bffc97496048 4
sgrsn 2:62821df48957 5 QEI::QEI(PinName A, PinName B, int ppr) : channelA(A), channelB(B), channelZ(NC)
sgrsn 2:62821df48957 6 {
sgrsn 2:62821df48957 7 Timer tmp;
sgrsn 2:62821df48957 8 _timer = &tmp;
sgrsn 2:62821df48957 9 _ppr = ppr;
sgrsn 2:62821df48957 10 init();
sgrsn 2:62821df48957 11 }
sgrsn 2:62821df48957 12
sgrsn 2:62821df48957 13 QEI::QEI(PinName A, PinName B, PinName Z, int ppr) : channelA(A), channelB(B), channelZ(Z)
sgrsn 2:62821df48957 14 {
sgrsn 2:62821df48957 15 Timer tmp;
sgrsn 2:62821df48957 16 _timer = &tmp;
sgrsn 2:62821df48957 17 _ppr = ppr;
sgrsn 2:62821df48957 18 init();
sgrsn 2:62821df48957 19 channelZ.rise(this, &QEI::encodeZ);
sgrsn 2:62821df48957 20 channelZ.fall(this, &QEI::encodeZ);
sgrsn 2:62821df48957 21 }
sgrsn 2:62821df48957 22
sgrsn 2:62821df48957 23 QEI::QEI(PinName A, PinName B, int ppr, Timer *timer) : channelA(A), channelB(B), channelZ(NC)
sgrsn 2:62821df48957 24 {
sgrsn 2:62821df48957 25 _timer = timer;
sgrsn 2:62821df48957 26 _ppr = ppr;
sgrsn 2:62821df48957 27 init();
sgrsn 2:62821df48957 28 }
sgrsn 2:62821df48957 29
sgrsn 2:62821df48957 30 QEI::QEI(PinName A, PinName B, PinName Z, int ppr, Timer *timer) : channelA(A), channelB(B), channelZ(Z)
sgrsn 2:62821df48957 31 {
sgrsn 2:62821df48957 32 _timer = timer;
sgrsn 2:62821df48957 33 _ppr = ppr;
sgrsn 2:62821df48957 34 init();
sgrsn 2:62821df48957 35 channelZ.rise(this, &QEI::encodeZ);
sgrsn 2:62821df48957 36 channelZ.fall(this, &QEI::encodeZ);
sgrsn 2:62821df48957 37 }
sgrsn 2:62821df48957 38
sgrsn 2:62821df48957 39 void QEI::init()
sgrsn 0:bffc97496048 40 {
sgrsn 0:bffc97496048 41 channelA.rise(this, &QEI::encode);
sgrsn 0:bffc97496048 42 channelB.rise(this, &QEI::encode);
sgrsn 0:bffc97496048 43 channelA.fall(this, &QEI::encode);
sgrsn 0:bffc97496048 44 channelB.fall(this, &QEI::encode);
sgrsn 0:bffc97496048 45 currState = 0;
sgrsn 0:bffc97496048 46 prevState = 0;
sgrsn 0:bffc97496048 47 position = 0;
sgrsn 2:62821df48957 48 _IsInterrupt = false;
sgrsn 2:62821df48957 49
sgrsn 2:62821df48957 50 _timer -> start();
sgrsn 2:62821df48957 51 }
sgrsn 2:62821df48957 52
sgrsn 2:62821df48957 53 float QEI::getDegree()
sgrsn 2:62821df48957 54 {
sgrsn 2:62821df48957 55 return float(position) * 360.0/(_ppr*4.0);
sgrsn 0:bffc97496048 56 }
sgrsn 0:bffc97496048 57
sgrsn 2:62821df48957 58 float QEI::getSpeed()
sgrsn 0:bffc97496048 59 {
sgrsn 2:62821df48957 60 static float last_time = 0;
sgrsn 2:62821df48957 61 static float last_degree = 0;
sgrsn 2:62821df48957 62 float current_time =_timer -> read();
sgrsn 2:62821df48957 63 float dt = current_time - last_time;
sgrsn 2:62821df48957 64 float current_degree = float(position) * 360.0/(_ppr*4.0);
sgrsn 2:62821df48957 65 float speed = (current_degree - last_degree) / dt;
sgrsn 2:62821df48957 66 last_degree = current_degree;
sgrsn 2:62821df48957 67 last_time = current_time;
sgrsn 2:62821df48957 68 return speed;
sgrsn 2:62821df48957 69 }
sgrsn 2:62821df48957 70
sgrsn 2:62821df48957 71 bool QEI::IsInterruptZ()
sgrsn 2:62821df48957 72 {
sgrsn 2:62821df48957 73 bool tmp = _IsInterrupt;
sgrsn 2:62821df48957 74 _IsInterrupt = false;
sgrsn 2:62821df48957 75 return tmp;
sgrsn 0:bffc97496048 76 }
sgrsn 0:bffc97496048 77
sgrsn 0:bffc97496048 78 void QEI::encode(void)
sgrsn 0:bffc97496048 79 {
sgrsn 0:bffc97496048 80 int8_t chanA = channelA.read();
sgrsn 0:bffc97496048 81 int8_t chanB = channelB.read();
sgrsn 0:bffc97496048 82 currState = chanA | (chanB << 1);
sgrsn 0:bffc97496048 83
sgrsn 0:bffc97496048 84 if (prevState != currState) {
sgrsn 0:bffc97496048 85 position += encodeTable[currState | (prevState<<2)];
sgrsn 0:bffc97496048 86 prevState = currState;
sgrsn 0:bffc97496048 87 }
sgrsn 2:62821df48957 88 }
sgrsn 2:62821df48957 89
sgrsn 2:62821df48957 90 void QEI::encodeZ()
sgrsn 2:62821df48957 91 {
sgrsn 2:62821df48957 92 _IsInterrupt = true;
sgrsn 0:bffc97496048 93 }