Ruprecht Altenburger / Misc_Lib

.

Dependents:  

DiffCounter.cpp@11:78e723ede0c6, 2019-02-25 (annotated)

Committer:
altb
Date:
Mon Feb 25 09:47:04 2019 +0000
Revision:
11:78e723ede0c6
Parent:
0:d784b08f51ff 
2018

Who changed what in which revision?

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