Ruprecht Altenburger / Lib_Misc

Miscellaneous Library, read Encoder etc.

Dependents:   My_Libraries

DiffCounter.cpp@2:1c5c71a6fac9, 2019-03-06 (annotated)

Committer:
altb
Date:
Wed Mar 06 14:38:04 2019 +0000
Revision:
2:1c5c71a6fac9
Parent:
0:3312872854c4 
.....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:3312872854c4 1 /*
altb 0:3312872854c4 2 DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
altb 0:3312872854c4 3
altb 0:3312872854c4 4 b*(1 - z^-1) s
altb 0:3312872854c4 5 G(z) = ------------- <-- tustin -- ----------- = G(s)
altb 0:3312872854c4 6 1 - a*z^-1 T*s + 1
altb 0:3312872854c4 7 */
altb 0:3312872854c4 8
altb 0:3312872854c4 9 #include "DiffCounter.h"
altb 0:3312872854c4 10 #define pi 3.141592653589793
altb 0:3312872854c4 11 using namespace std;
altb 0:3312872854c4 12
altb 0:3312872854c4 13 DiffCounter::DiffCounter(float T, float Ts)
altb 0:3312872854c4 14 {
altb 0:3312872854c4 15 b = 2.0/(2.0*(double)T + (double)Ts);
altb 0:3312872854c4 16 a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
altb 0:3312872854c4 17 incPast = 0;
altb 0:3312872854c4 18 vel = 0.0;
altb 0:3312872854c4 19 inc2rad = 2.0*pi/(4.0*6400.0); // incr encoder with 6400inc/rev
altb 0:3312872854c4 20 }
altb 0:3312872854c4 21
altb 0:3312872854c4 22 DiffCounter::~DiffCounter() {}
altb 0:3312872854c4 23
altb 0:3312872854c4 24 void DiffCounter::reset(float initValue, short inc)
altb 0:3312872854c4 25 {
altb 0:3312872854c4 26 vel = (double)initValue;
altb 0:3312872854c4 27 incPast = inc;
altb 0:3312872854c4 28 }
altb 0:3312872854c4 29
altb 0:3312872854c4 30 float DiffCounter::doStep(short inc)
altb 0:3312872854c4 31 {
altb 0:3312872854c4 32 long del = (long)(inc - incPast);
altb 0:3312872854c4 33 incPast = inc;
altb 0:3312872854c4 34 if(del < -16000)
altb 0:3312872854c4 35 del += 0xFFFF;
altb 0:3312872854c4 36 if(del > 16000)
altb 0:3312872854c4 37 del -= 0xFFFF;
altb 0:3312872854c4 38 vel = b*(double)del*inc2rad - a*vel;
altb 0:3312872854c4 39 return (float)vel;
altb 0:3312872854c4 40 }