Diff: Filter.cpp

Revision:

6:13ff4bea3c83

Parent:

5:ab6351c18264

diff -r ab6351c18264 -r 13ff4bea3c83 Filter.cpp
--- a/Filter.cpp	Tue Jan 16 15:56:33 2018 +0000
+++ b/Filter.cpp	Thu Mar 22 12:36:55 2018 +0000
@@ -25,16 +25,37 @@
     }
 }
 
+void Filter::setSecondOrderPara(double xOmega, double xDzeta, double init_data)
+{
+    omega = xOmega;
+    dzeta = xDzeta;
+    prev_output2 = prev_output1 = init_data;
+
+    set_secorder = true;
+}
+
+double Filter::SecondOrderLag(double input)
+{
+    if(!set_secorder) {
+        return input;
+    } else {
+        double output = (2*prev_output1*(1+dzeta*omega*int_time) - prev_output2 + pow(omega, 2.0)*pow(int_time, 2.0)*input)/(1 + 2*dzeta*omega*int_time + pow(omega, 2.0)*pow(int_time, 2.0));
+        prev_output2 = prev_output1;
+        prev_output1 = output;
+        return output;
+    }
+}
+
 void Filter::setNotchPara(double Omega, double int_data)
 {
     // 落としたい角周波数[rad/s]をOm_nに入れる
     Om_n = Omega;
     sq_Om = pow(Om_n, 2.0); // Om_nの2乗
     sq_dt = pow(int_time, 2.0); // dtの2乗
-    
+
     n_preOutput[0] = int_data;
     n_preOutput[1] = int_data;
-    
+
     n_preInput[0] = int_data;
     n_preInput[1] = int_data;
 }