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?
User | Revision | Line number | New 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 | } |