Fertig
Dependencies: mbed
Fork of RT2_P3_students by
DiffCounter.cpp@3:769ce5f06d3e, 2018-04-09 (annotated)
- Committer:
- altb
- Date:
- Mon Apr 09 08:01:29 2018 +0000
- Revision:
- 3:769ce5f06d3e
- Parent:
- 0:78ca29b4c49e
Changes from pmic
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb | 3:769ce5f06d3e | 1 | /* |
altb | 3:769ce5f06d3e | 2 | DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping |
altb | 3:769ce5f06d3e | 3 | |
altb | 3:769ce5f06d3e | 4 | b*(1 - z^-1) s |
altb | 3:769ce5f06d3e | 5 | G(z) = ------------- <-- tustin -- ----------- = G(s) |
altb | 3:769ce5f06d3e | 6 | 1 - a*z^-1 T*s + 1 |
altb | 0:78ca29b4c49e | 7 | */ |
altb | 0:78ca29b4c49e | 8 | |
altb | 3:769ce5f06d3e | 9 | #include "DiffCounter.h" |
altb | 3:769ce5f06d3e | 10 | #define pi 3.141592653589793 |
altb | 3:769ce5f06d3e | 11 | using namespace std; |
altb | 0:78ca29b4c49e | 12 | |
altb | 3:769ce5f06d3e | 13 | DiffCounter::DiffCounter(float T, float Ts) |
altb | 3:769ce5f06d3e | 14 | { |
altb | 3:769ce5f06d3e | 15 | b = 2.0/(2.0*(double)T + (double)Ts); |
altb | 3:769ce5f06d3e | 16 | a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts); |
altb | 3:769ce5f06d3e | 17 | incPast = 0; |
altb | 3:769ce5f06d3e | 18 | vel = 0.0; |
altb | 3:769ce5f06d3e | 19 | inc2rad = 2.0*pi/(4.0*6400.0); // incr encoder with 6400inc/rev |
altb | 0:78ca29b4c49e | 20 | } |
altb | 0:78ca29b4c49e | 21 | |
altb | 3:769ce5f06d3e | 22 | DiffCounter::~DiffCounter() {} |
altb | 3:769ce5f06d3e | 23 | |
altb | 3:769ce5f06d3e | 24 | void DiffCounter::reset(float initValue, short inc) |
altb | 3:769ce5f06d3e | 25 | { |
altb | 3:769ce5f06d3e | 26 | vel = (double)initValue; |
altb | 3:769ce5f06d3e | 27 | incPast = inc; |
altb | 3:769ce5f06d3e | 28 | } |
altb | 3:769ce5f06d3e | 29 | |
altb | 3:769ce5f06d3e | 30 | float DiffCounter::doStep(short inc) |
altb | 3:769ce5f06d3e | 31 | { |
altb | 3:769ce5f06d3e | 32 | long del = (long)(inc - incPast); |
altb | 3:769ce5f06d3e | 33 | incPast = inc; |
altb | 0:78ca29b4c49e | 34 | if(del < -16000) |
altb | 0:78ca29b4c49e | 35 | del += 0xFFFF; |
altb | 0:78ca29b4c49e | 36 | if(del > 16000) |
altb | 0:78ca29b4c49e | 37 | del -= 0xFFFF; |
altb | 3:769ce5f06d3e | 38 | vel = b*(double)del*inc2rad - a*vel; |
altb | 3:769ce5f06d3e | 39 | return (float)vel; |
altb | 3:769ce5f06d3e | 40 | } |