Ruprecht Altenburger / Mbed 2 deprecated RT2_P3_students_G4

Template for group 4

Dependencies:   mbed

Fork of RT2_P3_students by RT2_P3_students

DiffCounter.cpp@2:769ce5f06d3e, 2018-04-09 (annotated)

Committer:
altb
Date:
Mon Apr 09 08:01:29 2018 +0000
Revision:
2:769ce5f06d3e
Parent:
0:78ca29b4c49e 
Changes from pmic

Who changed what in which revision?

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