Nathaniel Honka / Mbed 2 deprecated Motion-Control

Corrected header file include guards.

Dependencies:   FiniteStateMachine HipControl Knee LinearBlend1 LocalFileSystem_Read dataComm hapticFeedback initExoVars mbed Blend_Generator Brad_poly_gait Gait_Generator MM_gait Encoders IMUdriver

Fork of Motion Control by HEL's Angels

Filter/filter.cpp

Committer:
nathanhonka
Date:
2015-07-02
Revision:
54:9d0e871dd00a
Parent:
0:d38d627c922f

File content as of revision 54:9d0e871dd00a:

#include "mbed.h"
#include "filter.h"

filter::filter()
{
    memset(xv, 0,sizeof(xv)); //reset filter
    memset(yv,0,sizeof(yv)); //reset filter
    memset(xv1, 0,sizeof(xv)); //reset filter
    memset(yv1,0,sizeof(yv)); //reset filter
}


/**********************************************************
2nd Order Lowpass Butterworth Filter
**********************************************************/
/* Digital filter designed by mkfilter/mkshape/gencode   A.J. Fisher
   Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 2 -a 2.5000000000e-01 0.0000000000e+00 -l */
//2nd order 50Hz LP BW filter

float xv[NZEROS+1], yv[NPOLES+1];

float filter::Butterworth(float input)
{
    xv[0] = xv[1];
    xv[1] = xv[2];
    xv[2] = input/GAIN;
    yv[0] = yv[1];
    yv[1] = yv[2];
    yv[2] =   (xv[0] + xv[2]) + 2 * xv[1]+ ( -0.4128015981 * yv[0]) + ( 1.1429805025 * yv[1]);
    return yv[2];
}

/* Digital filter designed by mkfilter/mkshape/gencode   A.J. Fisher
   Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 2 -a 5.0000000000e-02 0.0000000000e+00 -l */
//2nd order 50Hz LP BW filter at 1kHz sample freq.

float xv1[NZEROS+1], yv1[NPOLES+1];

float filter::Butterworth_1K(float input)
{
    xv1[0] = xv1[1];
    xv1[1] = xv1[2];
    xv1[2] = input/GAIN1;
    yv1[0] = yv1[1];
    yv1[1] = yv1[2];
    yv1[2] =   (xv1[0] + xv1[2]) + 2 * xv1[1]+ ( -0.6413515381 * yv1[0]) + ( 1.5610180758 * yv1[1]);
    return yv1[2];
}