Bayu Dharmaputra / Mbed 2 deprecated Filter_EMG

For BIOROBOTICS PROJECT

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@1:3bae5ab25e20, 2015-10-21 (annotated)

Committer:
dbayuadi
Date:
Wed Oct 21 12:31:55 2015 +0000
Revision:
1:3bae5ab25e20
Parent:
0:8ccd4c66e07f
Child:
2:ac98d055a6cd
whatever

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 1:3bae5ab25e20 4 AnalogIn emg(A0);
dbayuadi 1:3bae5ab25e20 5 HIDScope scope(5);
dbayuadi 0:8ccd4c66e07f 6 Ticker get;
dbayuadi 0:8ccd4c66e07f 7 AnalogIn pot(A1);
dbayuadi 0:8ccd4c66e07f 8 Serial pc(USBTX, USBRX); // tx, rx
dbayuadi 1:3bae5ab25e20 9 const int N = 10;
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 1:3bae5ab25e20 15 double y_rect;
dbayuadi 0:8ccd4c66e07f 16 const double Fs = 500;
dbayuadi 0:8ccd4c66e07f 17 const double Ts = 0.002;
dbayuadi 1:3bae5ab25e20 18 double a;
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 1:3bae5ab25e20 36 const double gain_high = 0.981587;
dbayuadi 0:8ccd4c66e07f 37 const double b0_high = 1.0*gain_high;
dbayuadi 1:3bae5ab25e20 38 const double b1_high = -1.99999992457*gain_high;
dbayuadi 0:8ccd4c66e07f 39 const double b2_high = 1.0*gain_high;
dbayuadi 1:3bae5ab25e20 40 const double a1_high = -1.96306054568;
dbayuadi 1:3bae5ab25e20 41 const double a2_high = 0.96374056522;
dbayuadi 0:8ccd4c66e07f 42 double v1_high = 0, v2_high = 0;
dbayuadi 0:8ccd4c66e07f 43
dbayuadi 1:3bae5ab25e20 44 const double gain_low = 0.000561;
dbayuadi 0:8ccd4c66e07f 45 const double b0_low = 1.0*gain_low;
dbayuadi 1:3bae5ab25e20 46 const double b1_low = 2.000*gain_low;
dbayuadi 0:8ccd4c66e07f 47 const double b2_low = 1.0*gain_low;
dbayuadi 1:3bae5ab25e20 48 const double a1_low = -1.93191036285;
dbayuadi 1:3bae5ab25e20 49 const double a2_low = 0.93415335760;
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 1:3bae5ab25e20 68 double sliding_average(double u,const int f_N)
dbayuadi 1:3bae5ab25e20 69 {
dbayuadi 1:3bae5ab25e20 70 double f_x[f_N];
dbayuadi 1:3bae5ab25e20 71 double f_sum=0;
dbayuadi 1:3bae5ab25e20 72 f_x[1]=abs(u);
dbayuadi 1:3bae5ab25e20 73 for (int i=f_N; i>=1; i--){
dbayuadi 1:3bae5ab25e20 74 f_x[i]=f_x[i-1];
dbayuadi 1:3bae5ab25e20 75 }
dbayuadi 1:3bae5ab25e20 76
dbayuadi 1:3bae5ab25e20 77
dbayuadi 1:3bae5ab25e20 78 for (int i=f_N; i>=0; i--){
dbayuadi 1:3bae5ab25e20 79 f_sum=f_x[i]+f_sum;
dbayuadi 1:3bae5ab25e20 80 }
dbayuadi 1:3bae5ab25e20 81 a=f_sum/double(f_N);
dbayuadi 1:3bae5ab25e20 82 f_sum=0;
dbayuadi 1:3bae5ab25e20 83 return a;
dbayuadi 1:3bae5ab25e20 84 }
dbayuadi 1:3bae5ab25e20 85 double z1;
dbayuadi 0:8ccd4c66e07f 86 void scopeSend()
dbayuadi 0:8ccd4c66e07f 87 {
dbayuadi 0:8ccd4c66e07f 88 y_high = biquad(emg.read(), v1_high, v1_high, a1_high, a2_high, b0_high, b1_high, b2_high);
dbayuadi 0:8ccd4c66e07f 89 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 1:3bae5ab25e20 90 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 1:3bae5ab25e20 91 y_rect = fabs(y_notch_2);
dbayuadi 1:3bae5ab25e20 92 y_low = biquad(y_rect, v1_low, v1_low, a1_low, a2_low, b0_low, b1_low, b2_low);
dbayuadi 1:3bae5ab25e20 93 // z1 = 6*sliding_average(emg.read(), N); //moving average
dbayuadi 1:3bae5ab25e20 94 scope.set(0,emg.read());
dbayuadi 1:3bae5ab25e20 95 scope.set(1,y_high);
dbayuadi 1:3bae5ab25e20 96 scope.set(2,y_rect);
dbayuadi 1:3bae5ab25e20 97 scope.set(3,y_low);
dbayuadi 1:3bae5ab25e20 98 // scope.set(4,z1);
dbayuadi 1:3bae5ab25e20 99
dbayuadi 0:8ccd4c66e07f 100 scope.send();
dbayuadi 0:8ccd4c66e07f 101 }
dbayuadi 0:8ccd4c66e07f 102
dbayuadi 0:8ccd4c66e07f 103 int main()
dbayuadi 0:8ccd4c66e07f 104 {
dbayuadi 0:8ccd4c66e07f 105 get.attach(&fn_activate,Ts);
dbayuadi 0:8ccd4c66e07f 106
dbayuadi 0:8ccd4c66e07f 107 while (true) {
dbayuadi 0:8ccd4c66e07f 108
dbayuadi 0:8ccd4c66e07f 109 if(fn_go == true) {
dbayuadi 0:8ccd4c66e07f 110
dbayuadi 0:8ccd4c66e07f 111 scopeSend();
dbayuadi 0:8ccd4c66e07f 112
dbayuadi 0:8ccd4c66e07f 113
dbayuadi 0:8ccd4c66e07f 114 fn_go = false;
dbayuadi 0:8ccd4c66e07f 115 }
dbayuadi 0:8ccd4c66e07f 116 }
dbayuadi 0:8ccd4c66e07f 117 }