Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

Filter.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
0:5e4f1e288e2a 
DDSS

Who changed what in which revision?

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