File content as of revision 4:fc9412c8334e:

#include "Filter.h"

Filter::Filter(double xint_time)
{
    int_time = xint_time;
    set_t = false;
}

void Filter::setLowPassPara(double T, double init_data)
{
    T_LPF = T;
    preOutput = init_data;
    set_t = true;
}

double Filter::LowPassFilter(double input)
{
    //static double preOutput = input;
    if(!set_t) {
        return input;
    } else {
        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;
}