Diff: QEI.cpp

Revision:

2:62821df48957

Parent:

0:bffc97496048

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