RT2_P3_students
/
RT2_P3_students_G4
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
RT2_P3_students_G4
by 
Ruprecht Altenburger
Diff: DiffCounter.cpp
- Revision:
- 0:78ca29b4c49e
- Child:
- 2:769ce5f06d3e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp Tue Apr 03 08:47:41 2018 +0000
@@ -0,0 +1,35 @@
+#include "DiffCounter.h"
+
+using namespace std;
+/* 1/tau*(z-1)
+G(z) = ------------
+ z - a0
+*/
+
+DiffCounter::DiffCounter(float time_constant, float SampleTime){
+ Ts = SampleTime;
+ alpha = 1/time_constant; // scaling
+ a0 = -(1-Ts/time_constant); // a0=-exp(-Ts/tau) ~= -(1-Ts/tau)
+ inc_old = 0; // old values
+ v_old = 0; // " "
+ inc2rad = 2.0f*3.1415927f/(4.0f*6400.0); // incr encoder with 6400inc/rev
+ }
+
+DiffCounter::~DiffCounter() {}
+
+void DiffCounter::reset(float initValue,short inc) {
+ v_old = initValue;
+ inc_old = inc;
+}
+
+float DiffCounter::doStep(short inc){
+ del = (long)(inc - inc_old);
+ if(del < -16000)
+ del += 0xFFFF;
+ if(del > 16000)
+ del -= 0xFFFF;
+ float vel = alpha * (float)del * inc2rad - a0 * v_old;
+ v_old = vel;
+ inc_old = inc;
+ return vel;
+ }
\ No newline at end of file