Miki Nakaone / Filter

2018/04/04

Dependents:   TOUTEKI_all_mbed mbed_test_program 2019_tourobo_upper

Fork of Filter by Yuki Ueno

Filter.cpp@7:e208acaef28d, 2018-04-04 (annotated)

Committer:
m204517
Date:
Wed Apr 04 07:29:13 2018 +0000
Revision:
7:e208acaef28d
Parent:
6:13ff4bea3c83 
2018/04/04

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uenono 0:17706252d1ed 1 #include "Filter.h"
uenono 0:17706252d1ed 2
uenono 1:637f9a61b133 3 Filter::Filter(double xint_time)
uenono 0:17706252d1ed 4 {
uenono 1:637f9a61b133 5 int_time = xint_time;
uenono 3:987d5d78f863 6 set_t = false;
uenono 0:17706252d1ed 7 }
uenono 0:17706252d1ed 8
uenono 4:fc9412c8334e 9 void Filter::setLowPassPara(double T, double init_data)
uenono 0:17706252d1ed 10 {
uenono 0:17706252d1ed 11 T_LPF = T;
uenono 4:fc9412c8334e 12 preOutput = init_data;
uenono 3:987d5d78f863 13 set_t = true;
uenono 0:17706252d1ed 14 }
uenono 0:17706252d1ed 15
uenono 0:17706252d1ed 16 double Filter::LowPassFilter(double input)
uenono 0:17706252d1ed 17 {
uenono 3:987d5d78f863 18 //static double preOutput = input;
uenono 3:987d5d78f863 19 if(!set_t) {
uenono 3:987d5d78f863 20 return input;
uenono 3:987d5d78f863 21 } else {
uenono 3:987d5d78f863 22 double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time);
uenono 3:987d5d78f863 23 preOutput = Output;
uenono 3:987d5d78f863 24 return Output;
uenono 3:987d5d78f863 25 }
uenono 0:17706252d1ed 26 }
uenono 0:17706252d1ed 27
uenono 6:13ff4bea3c83 28 void Filter::setSecondOrderPara(double xOmega, double xDzeta, double init_data)
uenono 6:13ff4bea3c83 29 {
uenono 6:13ff4bea3c83 30 omega = xOmega;
uenono 6:13ff4bea3c83 31 dzeta = xDzeta;
uenono 6:13ff4bea3c83 32 prev_output2 = prev_output1 = init_data;
uenono 6:13ff4bea3c83 33
uenono 6:13ff4bea3c83 34 set_secorder = true;
uenono 6:13ff4bea3c83 35 }
uenono 6:13ff4bea3c83 36
m204517 7:e208acaef28d 37 void Filter::holdSecondOrderValue(double hold_data)
m204517 7:e208acaef28d 38 {
m204517 7:e208acaef28d 39 prev_output2 = prev_output1 = hold_data;
m204517 7:e208acaef28d 40 }
m204517 7:e208acaef28d 41
m204517 7:e208acaef28d 42
uenono 6:13ff4bea3c83 43 double Filter::SecondOrderLag(double input)
uenono 6:13ff4bea3c83 44 {
uenono 6:13ff4bea3c83 45 if(!set_secorder) {
uenono 6:13ff4bea3c83 46 return input;
uenono 6:13ff4bea3c83 47 } else {
uenono 6:13ff4bea3c83 48 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));
uenono 6:13ff4bea3c83 49 prev_output2 = prev_output1;
uenono 6:13ff4bea3c83 50 prev_output1 = output;
uenono 6:13ff4bea3c83 51 return output;
uenono 6:13ff4bea3c83 52 }
uenono 6:13ff4bea3c83 53 }
uenono 6:13ff4bea3c83 54
uenono 5:ab6351c18264 55 void Filter::setNotchPara(double Omega, double int_data)
uenono 0:17706252d1ed 56 {
uenono 0:17706252d1ed 57 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 58 Om_n = Omega;
uenono 0:17706252d1ed 59 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 2:a842c1a33e4f 60 sq_dt = pow(int_time, 2.0); // dtの2乗
uenono 6:13ff4bea3c83 61
uenono 5:ab6351c18264 62 n_preOutput[0] = int_data;
uenono 5:ab6351c18264 63 n_preOutput[1] = int_data;
uenono 6:13ff4bea3c83 64
uenono 5:ab6351c18264 65 n_preInput[0] = int_data;
uenono 5:ab6351c18264 66 n_preInput[1] = int_data;
uenono 0:17706252d1ed 67 }
uenono 0:17706252d1ed 68
uenono 0:17706252d1ed 69 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 70 {
uenono 5:ab6351c18264 71 double Output = (2*(1 + Om_n * int_time) * n_preOutput[0]-n_preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * n_preInput[0] + n_preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
uenono 3:987d5d78f863 72
uenono 5:ab6351c18264 73 n_preInput[1] = n_preInput[0];
uenono 5:ab6351c18264 74 n_preInput[0] = input;
uenono 3:987d5d78f863 75
uenono 5:ab6351c18264 76 n_preOutput[1] = n_preOutput[0];
uenono 5:ab6351c18264 77 n_preOutput[0] = Output;
uenono 3:987d5d78f863 78
uenono 0:17706252d1ed 79 return Output;
uenono 0:17706252d1ed 80 }