.
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 23:05:32 by 1.7.2