Diff: DiffCounter.cpp

Revision:

0:8ab621116ccd

diff -r 000000000000 -r 8ab621116ccd DiffCounter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp	Tue Apr 03 15:17:11 2018 +0000
@@ -0,0 +1,35 @@
+#include "DiffCounter.h"
+
+using namespace std;
+/*      1/tau*(z-1)        
+G(z) = ------------
+         z - a0
+*/
+
+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
+    }
+
+DiffCounter::~DiffCounter() {} 
+    
+void DiffCounter::reset(float initValue,short inc) {
+    v_old = initValue;
+    inc_old = inc;
+}
+
+float DiffCounter::doStep(short inc){
+    del = (long)(inc - inc_old);
+    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