![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertig
Dependencies: mbed
Fork of RT2_P3_students by
Diff: DiffCounter.cpp
- Revision:
- 0:78ca29b4c49e
- Child:
- 3:769ce5f06d3e
diff -r 000000000000 -r 78ca29b4c49e DiffCounter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DiffCounter.cpp Tue Apr 03 08:47:41 2018 +0000 @@ -0,0 +1,35 @@ +#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; + } \ No newline at end of file