![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fertig
Dependencies: mbed
Fork of RT2_P3_students by
DiffCounter.cpp
- Committer:
- altb
- Date:
- 2018-04-03
- Revision:
- 0:78ca29b4c49e
- Child:
- 3:769ce5f06d3e
File content as of revision 0:78ca29b4c49e:
#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; }