BA / Mbed 2 deprecated RT2_P3_DAC_test

sfbsg

Dependencies:   mbed

DiffCounter.cpp@0:8ab621116ccd, 2018-04-03 (annotated)

Committer:
borlanic
Date:
Tue Apr 03 15:17:11 2018 +0000
Revision:
0:8ab621116ccd
fg

Who changed what in which revision?

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