Diff: QEI.cpp

Revision:

4:17d461de6a8f

Parent:

3:0035b165ecc4

diff -r 0035b165ecc4 -r 17d461de6a8f QEI.cpp
--- a/QEI.cpp	Fri Aug 31 17:09:18 2018 +0000
+++ b/QEI.cpp	Fri Apr 19 23:03:41 2019 +0000
@@ -1,3 +1,4 @@
+
 /**
  * @author Aaron Berk
  *
@@ -139,7 +140,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 +287,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::getVelocity()
+{
+    getDistance(53.975);
+    velocity_ = (newDistance_-oldDistance_)/(ti.read()*1000);
+    //printf("Distance %f, Time %f, Velocity %f\r\n", newDistance_-oldDistance_, ti.read(), velocity_);
+    ti.reset();
+    return velocity_;
+}