Ruprecht Altenburger / Library_Misc

2nd try

Dependents:   cuboid_balance

DiffCounter.cpp@3:29602f4ade5c, 2021-02-25 (annotated)

Committer:
altb2
Date:
Thu Feb 25 20:28:16 2021 +0000
Revision:
3:29602f4ade5c
Parent:
0:72b60c5271cc 
First commit of Mirror actuato, still under construction, pins should be ok, next: check path planner;

Who changed what in which revision?

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