Diff: DiffCounter.cpp

Revision:

12:6287235b2570

Parent:

8:d68e177e2571

diff -r ed2638662dfa -r 6287235b2570 DiffCounter.cpp
--- a/DiffCounter.cpp	Wed Apr 04 17:20:55 2018 +0000
+++ b/DiffCounter.cpp	Thu Apr 05 08:28:32 2018 +0000
@@ -7,22 +7,23 @@
 */
 
 #include "DiffCounter.h"
+#define   pi 3.141592653589793
 using namespace std;
 
 DiffCounter::DiffCounter(float T, float Ts)
 {   
-    b = 2.0f/(2.0f*T + Ts);
-    a = -(2.0f*T - Ts)/(2.0f*T + 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.0f;
-    inc2rad = 2.0f*3.1415927f/(4.0f*6400.0f);   // incr encoder with 6400inc/rev
+    vel = 0.0;
+    inc2rad = 2.0*pi/(4.0*6400.0);   // incr encoder with 6400inc/rev
 }
 
 DiffCounter::~DiffCounter() {}
 
 void DiffCounter::reset(float initValue, short inc)
 {
-    vel = initValue;
+    vel = (double)initValue;
     incPast = inc;
 }
 
@@ -34,6 +35,6 @@
         del += 0xFFFF;
     if(del > 16000)
         del -= 0xFFFF;
-    vel = b*(float)del*inc2rad - a*vel;
-    return vel;
+    vel = b*(double)del*inc2rad - a*vel;
+    return (float)vel;
 }
\ No newline at end of file