Diff: QEI.cpp

Revision:

1:6fa863d09d45

Parent:

0:bffc97496048

diff -r bffc97496048 -r 6fa863d09d45 QEI.cpp
--- a/QEI.cpp	Mon Aug 20 04:54:24 2018 +0000
+++ b/QEI.cpp	Wed Nov 14 02:18:58 2018 +0000
@@ -2,8 +2,9 @@
 
 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, Timer *t) : channelA(A), channelB(B)
 {
+    timer = t;
     channelA.rise(this, &QEI::encode);
     channelB.rise(this, &QEI::encode);
     channelA.fall(this, &QEI::encode);
@@ -12,6 +13,8 @@
     currState = 0;
     prevState = 0;
     position = 0;
+    last_position = 0;
+    time = timer->read();
 }
 
 float QEI::getAngle()
@@ -19,6 +22,29 @@
     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();