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

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