Revision 6:13ff4bea3c83, committed 2018-03-22

Comitter:

uenono

Date:

Thu Mar 22 12:36:55 2018 +0000

Parent:

5:ab6351c18264

Commit message:

second-order-lag???

Changed in this revision

--- 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;
 }

--- a/Filter.h	Tue Jan 16 15:56:33 2018 +0000
+++ b/Filter.h	Thu Mar 22 12:36:55 2018 +0000
@@ -7,11 +7,17 @@
     double Om_n;
     double sq_dt;
     double sq_Om;
-
+    
+    double omega;
+    double dzeta;
 
     Filter(double);
     void setLowPassPara(double T, double init_data);
     double LowPassFilter(double input);
+    
+    void setSecondOrderPara(double xOmega, double xDzeta, double init_data);
+    double SecondOrderLag(double input);
+    
     void setNotchPara(double Omega, double init_data);
     double NotchFilter(double input);
     
@@ -20,6 +26,9 @@
     double preOutput;
     bool set_t;
     
+    double prev_output1, prev_output2;
+    bool set_secorder;
+    
     double n_preOutput[2];
     double n_preInput[2];