altb_pmic / Lib_Misc

Lib_Misc

Dependents:   IndNav_QK3_T265

DiffCounter.cpp@14:d5f47a30ef19, 2020-01-10 (annotated)

Committer:
altb2
Date:
Fri Jan 10 16:01:33 2020 +0000
Revision:
14:d5f47a30ef19
Parent:
0:3312872854c4 
Small changes in double2float

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 }