Fertig
Dependencies: mbed
Fork of RT2_P3_students by
DiffCounter.cpp@0:78ca29b4c49e, 2018-04-03 (annotated)
- Committer:
- altb
- Date:
- Tue Apr 03 08:47:41 2018 +0000
- Revision:
- 0:78ca29b4c49e
- Child:
- 3:769ce5f06d3e
Cuboid Lab RT2 FS 2018
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 0:78ca29b4c49e | 1 | #include "DiffCounter.h" |
altb | 0:78ca29b4c49e | 2 | |
altb | 0:78ca29b4c49e | 3 | using namespace std; |
altb | 0:78ca29b4c49e | 4 | /* 1/tau*(z-1) |
altb | 0:78ca29b4c49e | 5 | G(z) = ------------ |
altb | 0:78ca29b4c49e | 6 | z - a0 |
altb | 0:78ca29b4c49e | 7 | */ |
altb | 0:78ca29b4c49e | 8 | |
altb | 0:78ca29b4c49e | 9 | DiffCounter::DiffCounter(float time_constant, float SampleTime){ |
altb | 0:78ca29b4c49e | 10 | Ts = SampleTime; |
altb | 0:78ca29b4c49e | 11 | alpha = 1/time_constant; // scaling |
altb | 0:78ca29b4c49e | 12 | a0 = -(1-Ts/time_constant); // a0=-exp(-Ts/tau) ~= -(1-Ts/tau) |
altb | 0:78ca29b4c49e | 13 | inc_old = 0; // old values |
altb | 0:78ca29b4c49e | 14 | v_old = 0; // " " |
altb | 0:78ca29b4c49e | 15 | inc2rad = 2.0f*3.1415927f/(4.0f*6400.0); // incr encoder with 6400inc/rev |
altb | 0:78ca29b4c49e | 16 | } |
altb | 0:78ca29b4c49e | 17 | |
altb | 0:78ca29b4c49e | 18 | DiffCounter::~DiffCounter() {} |
altb | 0:78ca29b4c49e | 19 | |
altb | 0:78ca29b4c49e | 20 | void DiffCounter::reset(float initValue,short inc) { |
altb | 0:78ca29b4c49e | 21 | v_old = initValue; |
altb | 0:78ca29b4c49e | 22 | inc_old = inc; |
altb | 0:78ca29b4c49e | 23 | } |
altb | 0:78ca29b4c49e | 24 | |
altb | 0:78ca29b4c49e | 25 | float DiffCounter::doStep(short inc){ |
altb | 0:78ca29b4c49e | 26 | del = (long)(inc - inc_old); |
altb | 0:78ca29b4c49e | 27 | if(del < -16000) |
altb | 0:78ca29b4c49e | 28 | del += 0xFFFF; |
altb | 0:78ca29b4c49e | 29 | if(del > 16000) |
altb | 0:78ca29b4c49e | 30 | del -= 0xFFFF; |
altb | 0:78ca29b4c49e | 31 | float vel = alpha * (float)del * inc2rad - a0 * v_old; |
altb | 0:78ca29b4c49e | 32 | v_old = vel; |
altb | 0:78ca29b4c49e | 33 | inc_old = inc; |
altb | 0:78ca29b4c49e | 34 | return vel; |
altb | 0:78ca29b4c49e | 35 | } |