DiffCounter.cpp

00001 /*  
00002     DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
00003     
00004               b*(1 - z^-1)                      s
00005       G(z) = -------------  <-- tustin --  ----------- = G(s)
00006               1 - a*z^-1                      T*s + 1
00007 */
00008 
00009 #include "DiffCounter.h"
00010 #define   pi 3.141592653589793
00011 using namespace std;
00012 
00013 DiffCounter::DiffCounter(float T, float Ts)
00014 {   
00015     b = 2.0/(2.0*(double)T + (double)Ts);
00016     a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
00017     incPast = 0;
00018     vel = 0.0;
00019     inc2rad = 2.0*pi/(4.0*6400.0);   // incr encoder with 6400inc/rev
00020 }
00021 
00022 DiffCounter::~DiffCounter() {}
00023 
00024 void DiffCounter::reset(float initValue, short inc)
00025 {
00026     vel = (double)initValue;
00027     incPast = inc;
00028 }
00029 
00030 float DiffCounter::doStep(short inc)
00031 {
00032     long del = (long)(inc - incPast);
00033     incPast = inc;
00034     if(del < -16000)
00035         del += 0xFFFF;
00036     if(del > 16000)
00037         del -= 0xFFFF;
00038     vel = b*(double)del*inc2rad - a*vel;
00039     return (float)vel;
00040 }