RT2_Cuboid_demo / Mbed 2 deprecated nucf446-cuboid-balance1_strong

Cuboid

Dependencies:   mbed

DiffCounter.cpp@8:d68e177e2571, 2018-03-22 (annotated)

Committer:
pmic
Date:
Thu Mar 22 17:32:37 2018 +0000
Revision:
8:d68e177e2571
Parent:
5:d6c7ccbbce78
Child:
12:6287235b2570
one day investigation in bugfixing of the hole software etc. for "austellung 23.03.2018", velocity still not zero while standing, still initial controller from stdl->altb->pmic, an other controller should be developed

Who changed what in which revision?

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