Diff: PosVelFilter.cpp

Revision:

0:82f27e7e99f0

Child:

1:ec4117689673

diff -r 000000000000 -r 82f27e7e99f0 PosVelFilter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PosVelFilter.cpp	Thu Apr 27 13:15:55 2017 +0000
@@ -0,0 +1,84 @@
+#include "PosVelFilter.hpp"
+#include "StaticDefs.hpp"
+#include "conversions.hpp"
+
+PosVelFilter::PosVelFilter()
+{
+    x1 = 0;
+    x2 = 0;
+    //w_n is the natural frequency of the filter bigger increases frequency response
+    w_n = 5.0;
+
+}
+
+void PosVelFilter::update()
+{
+    //run the pos/vel estimate filter
+    //this derives the timing from last run
+    last_time = curr_time;
+    curr_time = systemTime().read();
+    dt = curr_time-last_time;
+
+    // fetch the current distance reading from adc and convert to mm
+    conv_distance = counts_to_dist(adc().ch0_filt);
+
+    x1_dot = x2;
+    x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;
+
+    position = x1;
+    velocity = x2;
+
+    x1 += x1_dot*dt;
+    x2 += x2_dot*dt;
+
+}
+
+void PosVelFilter::init()
+{
+    // run filter for 2 seconds on startup
+    float start = systemTime().read();
+    float time = start;
+    pc().printf("\n\rWarming Up \n\r");
+
+    while ((time - start) < 1.0) {
+        update();
+
+        pc().printf("%5.3f \r", velocity);
+        time = systemTime().read();
+    }
+    pc().printf("\n\r");
+    while (1) {
+        update();
+
+        if (abs(velocity) > 0.005) {
+            pc().printf("Waiting to stablize. velocity: %5.3f \r", velocity);
+
+        } else {
+            pc().printf("\n\rreading stabilized\n\r");
+            break;
+        }
+    }
+
+}
+
+
+float PosVelFilter::getPosition()
+{
+    return position;
+}
+
+float PosVelFilter::getVelocity()
+{
+    return velocity;
+}
+
+float PosVelFilter::getDt()
+{
+    return dt;
+}
+
+void PosVelFilter::writeWn(float wn)
+{
+    w_n = wn;
+    pc().printf("\n\rWn set to: %f", w_n);
+}
\ No newline at end of file