Diff: PosVelFilter.cpp

Revision:

2:992e774dc62a

Parent:

1:ec4117689673

Child:

3:8107bb13278b

--- a/PosVelFilter.cpp	Tue Jun 13 19:47:50 2017 +0000
+++ b/PosVelFilter.cpp	Mon Jul 17 14:24:27 2017 +0000
@@ -7,7 +7,10 @@
     x1 = 0;
     x2 = 0;
     //w_n is the natural frequency of the filter bigger increases frequency response
-    w_n = 5.0;
+    
+    //w_n = 5.0;  // larger number equals faster response
+    
+    w_n = 2.0;      //5.0 was way off
 
 }
 
@@ -21,12 +24,19 @@
 
     // fetch the current distance reading from adc and convert to mm
     conv_distance = counts_to_dist(adc().ch0_filt);
+    //conv_distance = counts_to_dist(300);  //testing
 
-    x1_dot = x2;
+    x1_dot = x2*1.0;
     x2_dot = (-2.0*w_n*x2)-(w_n*w_n)*x1+(w_n*w_n)*conv_distance;
 
     position = x1;
     velocity = x2;
+    
+    //JUST A TEST (TROY)
+    //position = dt;
+    //velocity = 0;   //velocity needs to be zero, stabilize
+    //conv_distance = 1234;
+    
 
     x1 += x1_dot*dt;
     x2 += x2_dot*dt;
@@ -81,4 +91,39 @@
 {
     w_n = wn;
     pc().printf("\n\rWn set to: %f", w_n);
+}
+
+float PosVelFilter::get_conv_distance()
+{
+    return conv_distance;
+}
+
+float PosVelFilter::get_curr_time()
+{
+    return curr_time;
+}
+
+float PosVelFilter::get_last_time()
+{
+    return last_time;
+}
+
+float PosVelFilter::get_x1_dot()
+{
+    return x1_dot;
+}
+
+float PosVelFilter::get_x2_dot()
+{
+    return x2_dot;
+}
+
+float PosVelFilter::get_x1()
+{
+    return x1;
+}
+
+float PosVelFilter::get_x2()
+{
+    return x2;
 }
\ No newline at end of file