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@0:78ca29b4c49e, 2018-04-03 (annotated)

Committer:
altb
Date:
Tue Apr 03 08:47:41 2018 +0000
Revision:
0:78ca29b4c49e
Child:
2:769ce5f06d3e
Cuboid Lab RT2 FS 2018

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb 0:78ca29b4c49e 1 #include "DiffCounter.h"
altb 0:78ca29b4c49e 2
altb 0:78ca29b4c49e 3 using namespace std;
altb 0:78ca29b4c49e 4 /* 1/tau*(z-1)
altb 0:78ca29b4c49e 5 G(z) = ------------
altb 0:78ca29b4c49e 6 z - a0
altb 0:78ca29b4c49e 7 */
altb 0:78ca29b4c49e 8
altb 0:78ca29b4c49e 9 DiffCounter::DiffCounter(float time_constant, float SampleTime){
altb 0:78ca29b4c49e 10 Ts = SampleTime;
altb 0:78ca29b4c49e 11 alpha = 1/time_constant; // scaling
altb 0:78ca29b4c49e 12 a0 = -(1-Ts/time_constant); // a0=-exp(-Ts/tau) ~= -(1-Ts/tau)
altb 0:78ca29b4c49e 13 inc_old = 0; // old values
altb 0:78ca29b4c49e 14 v_old = 0; // " "
altb 0:78ca29b4c49e 15 inc2rad = 2.0f*3.1415927f/(4.0f*6400.0); // incr encoder with 6400inc/rev
altb 0:78ca29b4c49e 16 }
altb 0:78ca29b4c49e 17
altb 0:78ca29b4c49e 18 DiffCounter::~DiffCounter() {}
altb 0:78ca29b4c49e 19
altb 0:78ca29b4c49e 20 void DiffCounter::reset(float initValue,short inc) {
altb 0:78ca29b4c49e 21 v_old = initValue;
altb 0:78ca29b4c49e 22 inc_old = inc;
altb 0:78ca29b4c49e 23 }
altb 0:78ca29b4c49e 24
altb 0:78ca29b4c49e 25 float DiffCounter::doStep(short inc){
altb 0:78ca29b4c49e 26 del = (long)(inc - inc_old);
altb 0:78ca29b4c49e 27 if(del < -16000)
altb 0:78ca29b4c49e 28 del += 0xFFFF;
altb 0:78ca29b4c49e 29 if(del > 16000)
altb 0:78ca29b4c49e 30 del -= 0xFFFF;
altb 0:78ca29b4c49e 31 float vel = alpha * (float)del * inc2rad - a0 * v_old;
altb 0:78ca29b4c49e 32 v_old = vel;
altb 0:78ca29b4c49e 33 inc_old = inc;
altb 0:78ca29b4c49e 34 return vel;
altb 0:78ca29b4c49e 35 }