RT2_P3 / Mbed 2 deprecated nucf446-cuboid-balance_strong

cuboid strong

Dependencies:   mbed

DiffCounter.cpp@2:38d9d1c3c85f, 2018-03-01 (annotated)

Committer:
altb
Date:
Thu Mar 01 10:33:19 2018 +0000
Revision:
2:38d9d1c3c85f
Parent:
0:15be70d21d7c 
Cuboid with lift up and down

Who changed what in which revision?

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