File content as of revision 2:38d9d1c3c85f:

#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;
    }