Diff: QEI.cpp

Revision:

4:dc96133b5129

Parent:

3:0035b165ecc4

diff -r 0035b165ecc4 -r dc96133b5129 QEI.cpp
--- a/QEI.cpp	Fri Aug 31 17:09:18 2018 +0000
+++ b/QEI.cpp	Sat Oct 27 10:09:39 2018 +0000
@@ -139,7 +139,8 @@
     revolutions_  = 0;
     pulsesPerRev_ = pulsesPerRev;
     encoding_     = encoding;
-
+    
+    ti.start();
     //Workout what the current state is.
     int chanA = channelA_.read();
     int chanB = channelB_.read();
@@ -285,12 +286,20 @@
 
 float QEI::getDistance(float diameter)
 {
-    
-    return getRevolutions()*diameter*3.1415926;
+    oldDistance_ = newDistance_;
+    newDistance_ = getRevolutions()*diameter*3.1415926;
+    return newDistance_; 
 }
 
 void QEI::index(void) {
 
     revolutions_++;
-
 }
+float QEI::getVelosity()
+{
+    getDistance(53.975);
+    velosity_ = ti.read()*(newDistance_-oldDistance_);
+    ti.reset();
+    return velosity_;
+}
+