Bayu Dharmaputra / Mbed 2 deprecated Filter_EMG

For BIOROBOTICS PROJECT

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@0:8ccd4c66e07f, 2015-10-07 (annotated)

Committer:
dbayuadi
Date:
Wed Oct 07 20:52:18 2015 +0000
Revision:
0:8ccd4c66e07f
Child:
1:3bae5ab25e20
Implementing high pass notch and low pass filter for EMG measurement

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dbayuadi 0:8ccd4c66e07f 1 #include "mbed.h"
dbayuadi 0:8ccd4c66e07f 2 #include "HIDScope.h"
dbayuadi 0:8ccd4c66e07f 3 #include "QEI.h"
dbayuadi 0:8ccd4c66e07f 4 AnalogIn emg(A0)
dbayuadi 0:8ccd4c66e07f 5 HIDScope scope(1);
dbayuadi 0:8ccd4c66e07f 6 Ticker get;
dbayuadi 0:8ccd4c66e07f 7 AnalogIn pot(A1);
dbayuadi 0:8ccd4c66e07f 8 Serial pc(USBTX, USBRX); // tx, rx
dbayuadi 0:8ccd4c66e07f 9
dbayuadi 0:8ccd4c66e07f 10 volatile bool fn_go = false;
dbayuadi 0:8ccd4c66e07f 11 double y_notch_1;
dbayuadi 0:8ccd4c66e07f 12 double y_notch_2;
dbayuadi 0:8ccd4c66e07f 13 double y_high;
dbayuadi 0:8ccd4c66e07f 14 double y_low;
dbayuadi 0:8ccd4c66e07f 15
dbayuadi 0:8ccd4c66e07f 16 const double Fs = 500;
dbayuadi 0:8ccd4c66e07f 17 const double Ts = 0.002;
dbayuadi 0:8ccd4c66e07f 18
dbayuadi 0:8ccd4c66e07f 19 const double gain_notch_1 = 0.912483;
dbayuadi 0:8ccd4c66e07f 20 const double b0_notch_1 = 1.0*gain_notch_1;
dbayuadi 0:8ccd4c66e07f 21 const double b1_notch_1 = -1.62829098849*gain_notch_1;
dbayuadi 0:8ccd4c66e07f 22 const double b2_notch_1 = 1.0*gain_notch_1;
dbayuadi 0:8ccd4c66e07f 23 const double a1_notch_1 = -1.49965530713;
dbayuadi 0:8ccd4c66e07f 24 const double a2_notch_1 = 0.90720693582;
dbayuadi 0:8ccd4c66e07f 25 double v1_notch_1 = 0, v2_notch_1 = 0;
dbayuadi 0:8ccd4c66e07f 26
dbayuadi 0:8ccd4c66e07f 27
dbayuadi 0:8ccd4c66e07f 28 const double gain_notch_2 = 1.000000;
dbayuadi 0:8ccd4c66e07f 29 const double b0_notch_2 = 1.0*gain_notch_2;
dbayuadi 0:8ccd4c66e07f 30 const double b1_notch_2 = -1.63220522905*gain_notch_2;
dbayuadi 0:8ccd4c66e07f 31 const double b2_notch_2 = 1.0*gain_notch_2;
dbayuadi 0:8ccd4c66e07f 32 const double a1_notch_2 = -1.61200955424;
dbayuadi 0:8ccd4c66e07f 33 const double a2_notch_2 = 0.91813588764;
dbayuadi 0:8ccd4c66e07f 34 double v1_notch_2 = 0, v2_notch_2 = 0;
dbayuadi 0:8ccd4c66e07f 35
dbayuadi 0:8ccd4c66e07f 36 const double gain_high = 0.956602;
dbayuadi 0:8ccd4c66e07f 37 const double b0_high = 1.0*gain_high;
dbayuadi 0:8ccd4c66e07f 38 const double b1_high = -1.99999962667*gain_high;
dbayuadi 0:8ccd4c66e07f 39 const double b2_high = 1.0*gain_high;
dbayuadi 0:8ccd4c66e07f 40 const double a1_high = -1.91152224950;
dbayuadi 0:8ccd4c66e07f 41 const double a2_high = 0.91532727281;
dbayuadi 0:8ccd4c66e07f 42 double v1_high = 0, v2_high = 0;
dbayuadi 0:8ccd4c66e07f 43
dbayuadi 0:8ccd4c66e07f 44 const double gain_low = 0.003428;
dbayuadi 0:8ccd4c66e07f 45 const double b0_low = 1.0*gain_low;
dbayuadi 0:8ccd4c66e07f 46 const double b1_low = 2.00000000000*gain_low;
dbayuadi 0:8ccd4c66e07f 47 const double b2_low = 1.0*gain_low;
dbayuadi 0:8ccd4c66e07f 48 const double a1_low = -1.86528269522;
dbayuadi 0:8ccd4c66e07f 49 const double a2_low = 0.87380140464;
dbayuadi 0:8ccd4c66e07f 50 double v1_low = 0, v2_low = 0;
dbayuadi 0:8ccd4c66e07f 51
dbayuadi 0:8ccd4c66e07f 52
dbayuadi 0:8ccd4c66e07f 53 double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
dbayuadi 0:8ccd4c66e07f 54 {
dbayuadi 0:8ccd4c66e07f 55 double v = u - a1*v1 - a2*v2;
dbayuadi 0:8ccd4c66e07f 56 double y = b0*v + b1*v1 + b2*v2;
dbayuadi 0:8ccd4c66e07f 57 v2 = v1;
dbayuadi 0:8ccd4c66e07f 58 v1 = v;
dbayuadi 0:8ccd4c66e07f 59 return y;
dbayuadi 0:8ccd4c66e07f 60 }
dbayuadi 0:8ccd4c66e07f 61
dbayuadi 0:8ccd4c66e07f 62 void fn_activate()
dbayuadi 0:8ccd4c66e07f 63 {
dbayuadi 0:8ccd4c66e07f 64
dbayuadi 0:8ccd4c66e07f 65 fn_go = true;
dbayuadi 0:8ccd4c66e07f 66 }
dbayuadi 0:8ccd4c66e07f 67
dbayuadi 0:8ccd4c66e07f 68 void scopeSend()
dbayuadi 0:8ccd4c66e07f 69 {
dbayuadi 0:8ccd4c66e07f 70 y_high = biquad(emg.read(), v1_high, v1_high, a1_high, a2_high, b0_high, b1_high, b2_high);
dbayuadi 0:8ccd4c66e07f 71 y_notch_1 = biquad(y_high, v1_notch_1, v1_notch_1, a1_notch_1,a2_notch_1,b0_notch_1, b1_notch_1, b2_notch_1);
dbayuadi 0:8ccd4c66e07f 72 y_notch_2 = biquad(y_notch_1, v1_notch_2, v1_notch_2, a1_notch_2, a2_notch_2, b0_notch_2, b1_notch_2, b2_notch_2);
dbayuadi 0:8ccd4c66e07f 73
dbayuadi 0:8ccd4c66e07f 74 y_low = biquad(y_high, v1_low, v1_low, a1_low, a2_low, b0_low, b1_low, b2_low);
dbayuadi 0:8ccd4c66e07f 75 scope.set(0,y_low);
dbayuadi 0:8ccd4c66e07f 76 scope.send();
dbayuadi 0:8ccd4c66e07f 77 }
dbayuadi 0:8ccd4c66e07f 78
dbayuadi 0:8ccd4c66e07f 79 int main()
dbayuadi 0:8ccd4c66e07f 80 {
dbayuadi 0:8ccd4c66e07f 81 get.attach(&fn_activate,Ts);
dbayuadi 0:8ccd4c66e07f 82
dbayuadi 0:8ccd4c66e07f 83 while (true) {
dbayuadi 0:8ccd4c66e07f 84
dbayuadi 0:8ccd4c66e07f 85 if(fn_go == true) {
dbayuadi 0:8ccd4c66e07f 86
dbayuadi 0:8ccd4c66e07f 87 scopeSend();
dbayuadi 0:8ccd4c66e07f 88
dbayuadi 0:8ccd4c66e07f 89
dbayuadi 0:8ccd4c66e07f 90 fn_go = false;
dbayuadi 0:8ccd4c66e07f 91 }
dbayuadi 0:8ccd4c66e07f 92 }
dbayuadi 0:8ccd4c66e07f 93 }