Preston Ernst / Mbed OS mirror_actuator_VT

-data logging revision

Dependencies:   FastPWM

Lib_Misc/DiffCounter.cpp@2:92c25cb669f4, 2021-08-24 (annotated)

Committer:
ernstpre
Date:
Tue Aug 24 08:51:13 2021 +0000
Revision:
2:92c25cb669f4
Parent:
0:d2e117716219 
Publish Commit 24/8/21

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:d2e117716219 1 /*
altb2 0:d2e117716219 2 DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
altb2 0:d2e117716219 3
altb2 0:d2e117716219 4 b*(1 - z^-1) s
altb2 0:d2e117716219 5 G(z) = ------------- <-- tustin -- ----------- = G(s)
altb2 0:d2e117716219 6 1 - a*z^-1 T*s + 1
altb2 0:d2e117716219 7 */
altb2 0:d2e117716219 8
altb2 0:d2e117716219 9 #include "DiffCounter.h"
altb2 0:d2e117716219 10 #define pi 3.141592653589793
altb2 0:d2e117716219 11 using namespace std;
altb2 0:d2e117716219 12
altb2 0:d2e117716219 13 DiffCounter::DiffCounter(float T, float Ts)
altb2 0:d2e117716219 14 {
altb2 0:d2e117716219 15 this->incPerRev = 4.0*1000.0; // incr encoder with 6400inc/rev
altb2 0:d2e117716219 16 this->direction = 1.0f;
altb2 0:d2e117716219 17
altb2 0:d2e117716219 18 b = 2.0/(2.0*(double)T + (double)Ts);
altb2 0:d2e117716219 19 a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
altb2 0:d2e117716219 20
altb2 0:d2e117716219 21 incPast = 0;
altb2 0:d2e117716219 22 vel = 0.0;
altb2 0:d2e117716219 23
altb2 0:d2e117716219 24 inc2rad = 2.0*pi/static_cast<double>(incPerRev);
altb2 0:d2e117716219 25 }
altb2 0:d2e117716219 26 DiffCounter::DiffCounter(float T, float Ts,uint32_t incPerRev)
altb2 0:d2e117716219 27 {
altb2 0:d2e117716219 28 this->incPerRev = incPerRev;
altb2 0:d2e117716219 29 this->direction = 1.0f;
altb2 0:d2e117716219 30
altb2 0:d2e117716219 31 b = 2.0/(2.0*static_cast<double>(T) + static_cast<double>(Ts));
altb2 0:d2e117716219 32 a = -(2.0*static_cast<double>(T) - static_cast<double>(Ts))/(2.0*static_cast<double>(T) + static_cast<double>(Ts));
altb2 0:d2e117716219 33
altb2 0:d2e117716219 34 incPast = 0;
altb2 0:d2e117716219 35 vel = 0.0;
altb2 0:d2e117716219 36
altb2 0:d2e117716219 37 inc2rad = 2.0*pi/static_cast<double>(incPerRev); // incr encoder with inc/rev
altb2 0:d2e117716219 38 }
altb2 0:d2e117716219 39 DiffCounter::DiffCounter(float T, float Ts,uint32_t incPerRev, float direction)
altb2 0:d2e117716219 40 {
altb2 0:d2e117716219 41 this->incPerRev = incPerRev;
altb2 0:d2e117716219 42 this->direction = direction;
altb2 0:d2e117716219 43
altb2 0:d2e117716219 44 b = 2.0/(2.0*static_cast<double>(T) + static_cast<double>(Ts));
altb2 0:d2e117716219 45 a = -(2.0*static_cast<double>(T) - static_cast<double>(Ts))/(2.0*static_cast<double>(T) + static_cast<double>(Ts));
altb2 0:d2e117716219 46
altb2 0:d2e117716219 47 incPast = 0;
altb2 0:d2e117716219 48
altb2 0:d2e117716219 49 vel = 0.0;
altb2 0:d2e117716219 50 inc2rad = 2.0*pi/static_cast<double>(incPerRev); // incr encoder with inc/rev
altb2 0:d2e117716219 51 }
altb2 0:d2e117716219 52 DiffCounter::~DiffCounter() {}
altb2 0:d2e117716219 53
altb2 0:d2e117716219 54 void DiffCounter::reset(float initValue, short inc)
altb2 0:d2e117716219 55 {
altb2 0:d2e117716219 56 vel = static_cast<double>(initValue);
altb2 0:d2e117716219 57 incPast = inc;
altb2 0:d2e117716219 58
altb2 0:d2e117716219 59 return;
altb2 0:d2e117716219 60 }
altb2 0:d2e117716219 61
altb2 0:d2e117716219 62 float DiffCounter::doStep(int16_t inc)
altb2 0:d2e117716219 63 {
altb2 0:d2e117716219 64 int16_t delta(inc - incPast);
altb2 0:d2e117716219 65 incPast = inc;
altb2 0:d2e117716219 66 vel = b*static_cast<double>(delta)*inc2rad - a*vel;
altb2 0:d2e117716219 67
altb2 0:d2e117716219 68 return direction*(static_cast<float>(vel));
altb2 0:d2e117716219 69 }