Diff: Filter.cpp

Revision:

0:17706252d1ed

Child:

1:637f9a61b133

diff -r 000000000000 -r 17706252d1ed Filter.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter.cpp	Thu Jan 11 06:44:59 2018 +0000
@@ -0,0 +1,47 @@
+#include "Filter.h"
+#include "define.h"
+
+Filter::Filter()
+{
+}
+
+void Filter::setLowPassPara(double T)
+{
+    T_LPF = T;
+}
+
+double Filter::LowPassFilter(double input)
+{
+    static double preOutput = 0.0;
+
+    double Output = (INT_TIME * input + T_LPF * preOutput)/(T_LPF + INT_TIME);
+
+    preOutput = Output;
+
+    return Output;
+}
+
+void Filter::setNotchPara(double Omega)
+{
+    // 落としたい角周波数[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乗
+
+}
+
+double Filter::NotchFilter(double input)
+{
+    static double preOutput[2] = {0.0, 0.0};
+    static double preInput[2] = {0.0, 0.0};
+    
+    double Output = (2*(1 + Om_n * INT_TIME) * preOutput[0]-preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * preInput[0] + preInput[1]) / (1 + 2 * Om_n * INT_TIME + sq_Om * sq_dt);
+    
+    preInput[1] = preInput[0];
+    preInput[0] = input;
+    
+    preOutput[1] = preOutput[0];
+    preOutput[0] = Output;
+    
+    return Output;
+}
\ No newline at end of file