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@0:d38d627c922f, 2014-11-14 (annotated)

Committer:
perr1940
Date:
Fri Nov 14 22:39:10 2014 +0000
Revision:
0:d38d627c922f
this is without encoders going to test the knee steppers alone.  They work alone but not with the encoders.  Strange huh?

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 }