James Kiwic
/
RT2_P3_students
Fertig
Fork of RT2_P3_students by
Diff: DiffCounter.cpp
- Revision:
- 3:769ce5f06d3e
- Parent:
- 0:78ca29b4c49e
diff -r a30512c3ac73 -r 769ce5f06d3e DiffCounter.cpp --- a/DiffCounter.cpp Mon Apr 09 05:50:04 2018 +0000 +++ b/DiffCounter.cpp Mon Apr 09 08:01:29 2018 +0000 @@ -1,35 +1,40 @@ -#include "DiffCounter.h" - -using namespace std; -/* 1/tau*(z-1) -G(z) = ------------ - z - a0 +/* + DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping + + b*(1 - z^-1) s + G(z) = ------------- <-- tustin -- ----------- = G(s) + 1 - a*z^-1 T*s + 1 */ -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 - } +#include "DiffCounter.h" +#define pi 3.141592653589793 +using namespace std; -DiffCounter::~DiffCounter() {} - -void DiffCounter::reset(float initValue,short inc) { - v_old = initValue; - inc_old = inc; +DiffCounter::DiffCounter(float T, float 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.0; + inc2rad = 2.0*pi/(4.0*6400.0); // incr encoder with 6400inc/rev } -float DiffCounter::doStep(short inc){ - del = (long)(inc - inc_old); +DiffCounter::~DiffCounter() {} + +void DiffCounter::reset(float initValue, short inc) +{ + vel = (double)initValue; + incPast = inc; +} + +float DiffCounter::doStep(short inc) +{ + long del = (long)(inc - incPast); + incPast = inc; 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; - } \ No newline at end of file + vel = b*(double)del*inc2rad - a*vel; + return (float)vel; +} \ No newline at end of file