Byron van Gool / Mbed 2 deprecated Filter_EMG_Fred_style

This is a test version

Dependencies:   HIDScope MODSERIAL QEI mbed

Fork of Filter_EMG by Bayu Dharmaputra

main.cpp@1:b9471e519760, 2015-10-18 (annotated)

Committer:
bvgool
Date:
Sun Oct 18 14:11:58 2015 +0000
Revision:
1:b9471e519760
Parent:
0:8ccd4c66e07f 
this is a test version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dbayuadi 0:8ccd4c66e07f 1 #include "mbed.h"
bvgool 1:b9471e519760 2 #include "MODSERIAL.h"
bvgool 1:b9471e519760 3 #include "math.h"
dbayuadi 0:8ccd4c66e07f 4 #include "HIDScope.h"
dbayuadi 0:8ccd4c66e07f 5
dbayuadi 0:8ccd4c66e07f 6
bvgool 1:b9471e519760 7 AnalogIn EMG(A1);
bvgool 1:b9471e519760 8 MODSERIAL pc(USBTX, USBRX);
bvgool 1:b9471e519760 9
bvgool 1:b9471e519760 10 //const int baudrate = 115200;
bvgool 1:b9471e519760 11 //const int ms_wait = 100;
bvgool 1:b9471e519760 12
bvgool 1:b9471e519760 13 // Define the HIDScope and Ticker objects
bvgool 1:b9471e519760 14 HIDScope scope(1);
bvgool 1:b9471e519760 15 Ticker scopeTimer;
bvgool 1:b9471e519760 16
bvgool 1:b9471e519760 17 // Define moving average variables
bvgool 1:b9471e519760 18 const int N = 125;
bvgool 1:b9471e519760 19 double a;
bvgool 1:b9471e519760 20
bvgool 1:b9471e519760 21 // Define the storage variables and filter coe icients for two filters
bvgool 1:b9471e519760 22 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0;
bvgool 1:b9471e519760 23 const double f1_a1 = -1.822694925196308, f1_a2 = 0.837181651256022, f1_b0 = 0.914969144113083, f1_b1 = -1.829938288226165, f1_b2 = 0.914969144113083;
bvgool 1:b9471e519760 24 const double f2_a1 = -1.142980502539901, f2_a2 = 0.412801598096189, f2_b0 = 0.067455273889072, f2_b1 = 0.134910547778144, f2_b2 = 0.067455273889072;
dbayuadi 0:8ccd4c66e07f 25
bvgool 1:b9471e519760 26 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
bvgool 1:b9471e519760 27 {
bvgool 1:b9471e519760 28 double v = u-a1*v1-a2*v2;
bvgool 1:b9471e519760 29 double y = b0*v + b1*v1 + b2*v2;
bvgool 1:b9471e519760 30 v2 = v1; v1 = v;
bvgool 1:b9471e519760 31 return y;
bvgool 1:b9471e519760 32 }
dbayuadi 0:8ccd4c66e07f 33
bvgool 1:b9471e519760 34 double sliding_average(double u,const int f_N)
bvgool 1:b9471e519760 35 {
bvgool 1:b9471e519760 36 double f_x[f_N];
bvgool 1:b9471e519760 37 double f_sum=0;
bvgool 1:b9471e519760 38 f_x[1]=abs(u);
bvgool 1:b9471e519760 39 for (int i=f_N; i>=1; i--){
bvgool 1:b9471e519760 40 f_x[i]=f_x[i-1];
bvgool 1:b9471e519760 41 }
bvgool 1:b9471e519760 42
bvgool 1:b9471e519760 43
bvgool 1:b9471e519760 44 for (int i=f_N; i>=0; i--){
bvgool 1:b9471e519760 45 f_sum=f_x[i]+f_sum;
bvgool 1:b9471e519760 46 }
bvgool 1:b9471e519760 47 a=f_sum/double(f_N);
bvgool 1:b9471e519760 48 f_sum=0;
bvgool 1:b9471e519760 49 return a;
bvgool 1:b9471e519760 50 }
dbayuadi 0:8ccd4c66e07f 51
dbayuadi 0:8ccd4c66e07f 52
dbayuadi 0:8ccd4c66e07f 53
dbayuadi 0:8ccd4c66e07f 54
bvgool 1:b9471e519760 55 void scopeSend(){
bvgool 1:b9471e519760 56 double u1 = EMG.read() ; //filter 1 input
bvgool 1:b9471e519760 57 double y1 = biquad( u1, f1_v1, f1_v2, f1_a1, f1_a2, f1_b0, f1_b1, f1_b2 ); //notch
bvgool 1:b9471e519760 58 double y2 = biquad( y1, f2_v1, f2_v2, f2_a1, f2_a2, f2_b0, f2_b1, f2_b2 ); //lowpass with rectifier
bvgool 1:b9471e519760 59 double z1 = 3*sliding_average(y2, N); //moving average
bvgool 1:b9471e519760 60 scope.set(0,z1);
bvgool 1:b9471e519760 61 scope.send();
bvgool 1:b9471e519760 62 }
bvgool 1:b9471e519760 63
dbayuadi 0:8ccd4c66e07f 64 int main()
dbayuadi 0:8ccd4c66e07f 65 {
bvgool 1:b9471e519760 66 scopeTimer.attach(&scopeSend,0.002);
bvgool 1:b9471e519760 67 while(1){}
dbayuadi 0:8ccd4c66e07f 68
dbayuadi 0:8ccd4c66e07f 69 }