Diff: DiffCounter.cpp

Revision:

3:769ce5f06d3e

Parent:

0:78ca29b4c49e

diff -r a30512c3ac73 -r 769ce5f06d3e DiffCounter.cpp
--- a/DiffCounter.cpp	Mon Apr 09 05:50:04 2018 +0000
+++ b/DiffCounter.cpp	Mon Apr 09 08:01:29 2018 +0000
@@ -1,35 +1,40 @@
-#include "DiffCounter.h"
-
-using namespace std;
-/*      1/tau*(z-1)        
-G(z) = ------------
-         z - a0
+/*  
+    DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
+    
+              b*(1 - z^-1)                      s
+      G(z) = -------------  <-- tustin --  ----------- = G(s)
+              1 - a*z^-1                      T*s + 1
 */
 
-DiffCounter::DiffCounter(float time_constant, float SampleTime){
-    Ts = SampleTime;
-    alpha = 1/time_constant;            // scaling
-    a0 = -(1-Ts/time_constant);         // a0=-exp(-Ts/tau) ~= -(1-Ts/tau) 
-    inc_old = 0;                        // old values
-    v_old = 0;                          //   "  "
-    inc2rad = 2.0f*3.1415927f/(4.0f*6400.0);   // incr encoder with 6400inc/rev
-    }
+#include "DiffCounter.h"
+#define   pi 3.141592653589793
+using namespace std;
 
-DiffCounter::~DiffCounter() {} 
-    
-void DiffCounter::reset(float initValue,short inc) {
-    v_old = initValue;
-    inc_old = inc;
+DiffCounter::DiffCounter(float T, float Ts)
+{   
+    b = 2.0/(2.0*(double)T + (double)Ts);
+    a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
+    incPast = 0;
+    vel = 0.0;
+    inc2rad = 2.0*pi/(4.0*6400.0);   // incr encoder with 6400inc/rev
 }
 
-float DiffCounter::doStep(short inc){
-    del = (long)(inc - inc_old);
+DiffCounter::~DiffCounter() {}
+
+void DiffCounter::reset(float initValue, short inc)
+{
+    vel = (double)initValue;
+    incPast = inc;
+}
+
+float DiffCounter::doStep(short inc)
+{
+    long del = (long)(inc - incPast);
+    incPast = inc;
     if(del < -16000)
         del += 0xFFFF;
     if(del > 16000)
         del -= 0xFFFF;
-    float vel = alpha * (float)del * inc2rad - a0 * v_old;
-    v_old = vel;
-    inc_old = inc;
-    return vel;
-    }
\ No newline at end of file
+    vel = b*(double)del*inc2rad - a*vel;
+    return (float)vel;
+}
\ No newline at end of file