gpa in double prescision

Dependencies:   mbed

Committer:
pmic
Date:
Tue Apr 10 12:25:03 2018 +0000
Revision:
28:c71b6856652e
Parent:
19:44760408b75e
correct new functinality

Who changed what in which revision?

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