Ruprecht Altenburger
/
mirror_actuator_preston
corrections altb2
Lib_Misc/DiffCounter.cpp@0:d2e117716219, 2021-05-02 (annotated)
- Committer:
- altb2
- Date:
- Sun May 02 19:32:30 2021 +0000
- Revision:
- 0:d2e117716219
1st commit;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:d2e117716219 | 1 | /* |
altb2 | 0:d2e117716219 | 2 | DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping |
altb2 | 0:d2e117716219 | 3 | |
altb2 | 0:d2e117716219 | 4 | b*(1 - z^-1) s |
altb2 | 0:d2e117716219 | 5 | G(z) = ------------- <-- tustin -- ----------- = G(s) |
altb2 | 0:d2e117716219 | 6 | 1 - a*z^-1 T*s + 1 |
altb2 | 0:d2e117716219 | 7 | */ |
altb2 | 0:d2e117716219 | 8 | |
altb2 | 0:d2e117716219 | 9 | #include "DiffCounter.h" |
altb2 | 0:d2e117716219 | 10 | #define pi 3.141592653589793 |
altb2 | 0:d2e117716219 | 11 | using namespace std; |
altb2 | 0:d2e117716219 | 12 | |
altb2 | 0:d2e117716219 | 13 | DiffCounter::DiffCounter(float T, float Ts) |
altb2 | 0:d2e117716219 | 14 | { |
altb2 | 0:d2e117716219 | 15 | this->incPerRev = 4.0*1000.0; // incr encoder with 6400inc/rev |
altb2 | 0:d2e117716219 | 16 | this->direction = 1.0f; |
altb2 | 0:d2e117716219 | 17 | |
altb2 | 0:d2e117716219 | 18 | b = 2.0/(2.0*(double)T + (double)Ts); |
altb2 | 0:d2e117716219 | 19 | a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts); |
altb2 | 0:d2e117716219 | 20 | |
altb2 | 0:d2e117716219 | 21 | incPast = 0; |
altb2 | 0:d2e117716219 | 22 | vel = 0.0; |
altb2 | 0:d2e117716219 | 23 | |
altb2 | 0:d2e117716219 | 24 | inc2rad = 2.0*pi/static_cast<double>(incPerRev); |
altb2 | 0:d2e117716219 | 25 | } |
altb2 | 0:d2e117716219 | 26 | DiffCounter::DiffCounter(float T, float Ts,uint32_t incPerRev) |
altb2 | 0:d2e117716219 | 27 | { |
altb2 | 0:d2e117716219 | 28 | this->incPerRev = incPerRev; |
altb2 | 0:d2e117716219 | 29 | this->direction = 1.0f; |
altb2 | 0:d2e117716219 | 30 | |
altb2 | 0:d2e117716219 | 31 | b = 2.0/(2.0*static_cast<double>(T) + static_cast<double>(Ts)); |
altb2 | 0:d2e117716219 | 32 | a = -(2.0*static_cast<double>(T) - static_cast<double>(Ts))/(2.0*static_cast<double>(T) + static_cast<double>(Ts)); |
altb2 | 0:d2e117716219 | 33 | |
altb2 | 0:d2e117716219 | 34 | incPast = 0; |
altb2 | 0:d2e117716219 | 35 | vel = 0.0; |
altb2 | 0:d2e117716219 | 36 | |
altb2 | 0:d2e117716219 | 37 | inc2rad = 2.0*pi/static_cast<double>(incPerRev); // incr encoder with inc/rev |
altb2 | 0:d2e117716219 | 38 | } |
altb2 | 0:d2e117716219 | 39 | DiffCounter::DiffCounter(float T, float Ts,uint32_t incPerRev, float direction) |
altb2 | 0:d2e117716219 | 40 | { |
altb2 | 0:d2e117716219 | 41 | this->incPerRev = incPerRev; |
altb2 | 0:d2e117716219 | 42 | this->direction = direction; |
altb2 | 0:d2e117716219 | 43 | |
altb2 | 0:d2e117716219 | 44 | b = 2.0/(2.0*static_cast<double>(T) + static_cast<double>(Ts)); |
altb2 | 0:d2e117716219 | 45 | a = -(2.0*static_cast<double>(T) - static_cast<double>(Ts))/(2.0*static_cast<double>(T) + static_cast<double>(Ts)); |
altb2 | 0:d2e117716219 | 46 | |
altb2 | 0:d2e117716219 | 47 | incPast = 0; |
altb2 | 0:d2e117716219 | 48 | |
altb2 | 0:d2e117716219 | 49 | vel = 0.0; |
altb2 | 0:d2e117716219 | 50 | inc2rad = 2.0*pi/static_cast<double>(incPerRev); // incr encoder with inc/rev |
altb2 | 0:d2e117716219 | 51 | } |
altb2 | 0:d2e117716219 | 52 | DiffCounter::~DiffCounter() {} |
altb2 | 0:d2e117716219 | 53 | |
altb2 | 0:d2e117716219 | 54 | void DiffCounter::reset(float initValue, short inc) |
altb2 | 0:d2e117716219 | 55 | { |
altb2 | 0:d2e117716219 | 56 | vel = static_cast<double>(initValue); |
altb2 | 0:d2e117716219 | 57 | incPast = inc; |
altb2 | 0:d2e117716219 | 58 | |
altb2 | 0:d2e117716219 | 59 | return; |
altb2 | 0:d2e117716219 | 60 | } |
altb2 | 0:d2e117716219 | 61 | |
altb2 | 0:d2e117716219 | 62 | float DiffCounter::doStep(int16_t inc) |
altb2 | 0:d2e117716219 | 63 | { |
altb2 | 0:d2e117716219 | 64 | int16_t delta(inc - incPast); |
altb2 | 0:d2e117716219 | 65 | incPast = inc; |
altb2 | 0:d2e117716219 | 66 | vel = b*static_cast<double>(delta)*inc2rad - a*vel; |
altb2 | 0:d2e117716219 | 67 | |
altb2 | 0:d2e117716219 | 68 | return direction*(static_cast<float>(vel)); |
altb2 | 0:d2e117716219 | 69 | } |