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@54:9d0e871dd00a, 2015-07-02 (annotated)

Committer:
nathanhonka
Date:
Thu Jul 02 21:42:11 2015 +0000
Revision:
54:9d0e871dd00a
Parent:
0:d38d627c922f 
Corrected header file include guards.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
perr1940 0:d38d627c922f 1 #include "mbed.h"
perr1940 0:d38d627c922f 2 #include "filter.h"
perr1940 0:d38d627c922f 3
perr1940 0:d38d627c922f 4 filter::filter()
perr1940 0:d38d627c922f 5 {
perr1940 0:d38d627c922f 6 memset(xv, 0,sizeof(xv)); //reset filter
perr1940 0:d38d627c922f 7 memset(yv,0,sizeof(yv)); //reset filter
perr1940 0:d38d627c922f 8 memset(xv1, 0,sizeof(xv)); //reset filter
perr1940 0:d38d627c922f 9 memset(yv1,0,sizeof(yv)); //reset filter
perr1940 0:d38d627c922f 10 }
perr1940 0:d38d627c922f 11
perr1940 0:d38d627c922f 12
perr1940 0:d38d627c922f 13 /**********************************************************
perr1940 0:d38d627c922f 14 2nd Order Lowpass Butterworth Filter
perr1940 0:d38d627c922f 15 **********************************************************/
perr1940 0:d38d627c922f 16 /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher
perr1940 0:d38d627c922f 17 Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 2 -a 2.5000000000e-01 0.0000000000e+00 -l */
perr1940 0:d38d627c922f 18 //2nd order 50Hz LP BW filter
perr1940 0:d38d627c922f 19
perr1940 0:d38d627c922f 20 float xv[NZEROS+1], yv[NPOLES+1];
perr1940 0:d38d627c922f 21
perr1940 0:d38d627c922f 22 float filter::Butterworth(float input)
perr1940 0:d38d627c922f 23 {
perr1940 0:d38d627c922f 24 xv[0] = xv[1];
perr1940 0:d38d627c922f 25 xv[1] = xv[2];
perr1940 0:d38d627c922f 26 xv[2] = input/GAIN;
perr1940 0:d38d627c922f 27 yv[0] = yv[1];
perr1940 0:d38d627c922f 28 yv[1] = yv[2];
perr1940 0:d38d627c922f 29 yv[2] = (xv[0] + xv[2]) + 2 * xv[1]+ ( -0.4128015981 * yv[0]) + ( 1.1429805025 * yv[1]);
perr1940 0:d38d627c922f 30 return yv[2];
perr1940 0:d38d627c922f 31 }
perr1940 0:d38d627c922f 32
perr1940 0:d38d627c922f 33 /* Digital filter designed by mkfilter/mkshape/gencode A.J. Fisher
perr1940 0:d38d627c922f 34 Command line: /www/usr/fisher/helpers/mkfilter -Bu -Lp -o 2 -a 5.0000000000e-02 0.0000000000e+00 -l */
perr1940 0:d38d627c922f 35 //2nd order 50Hz LP BW filter at 1kHz sample freq.
perr1940 0:d38d627c922f 36
perr1940 0:d38d627c922f 37 float xv1[NZEROS+1], yv1[NPOLES+1];
perr1940 0:d38d627c922f 38
perr1940 0:d38d627c922f 39 float filter::Butterworth_1K(float input)
perr1940 0:d38d627c922f 40 {
perr1940 0:d38d627c922f 41 xv1[0] = xv1[1];
perr1940 0:d38d627c922f 42 xv1[1] = xv1[2];
perr1940 0:d38d627c922f 43 xv1[2] = input/GAIN1;
perr1940 0:d38d627c922f 44 yv1[0] = yv1[1];
perr1940 0:d38d627c922f 45 yv1[1] = yv1[2];
perr1940 0:d38d627c922f 46 yv1[2] = (xv1[0] + xv1[2]) + 2 * xv1[1]+ ( -0.6413515381 * yv1[0]) + ( 1.5610180758 * yv1[1]);
perr1940 0:d38d627c922f 47 return yv1[2];
perr1940 0:d38d627c922f 48 }