DiffCounter.cpp

00001 #include "DiffCounter.h"
00002 
00003 using namespace std;
00004 /*      1/tau*(z-1)        
00005 G(z) = ------------
00006          z - a0
00007 */
00008 
00009 DiffCounter::DiffCounter(float time_constant, float SampleTime){
00010     Ts = SampleTime;
00011     alpha = 1/time_constant;            // scaling
00012     a0 = -(1-Ts/time_constant);         // a0=-exp(-Ts/tau) ~= -(1-Ts/tau) 
00013     inc_old = 0;                        // old values
00014     v_old = 0;                          //   "  "
00015     inc2rad = 2.0f*3.1415927f/(4.0f*6400.0);   // incr encoder with 6400inc/rev
00016     }
00017 
00018 DiffCounter::~DiffCounter() {} 
00019     
00020 void DiffCounter::reset(float initValue,short inc) {
00021     v_old = initValue;
00022     inc_old = inc;
00023 }
00024 
00025 float DiffCounter::doStep(short inc){
00026     del = (long)(inc - inc_old);
00027     if(del < -16000)
00028         del += 0xFFFF;
00029     if(del > 16000)
00030         del -= 0xFFFF;
00031     float vel = alpha * (float)del * inc2rad - a0 * v_old;
00032     v_old = vel;
00033     inc_old = inc;
00034     return vel;
00035     }