Revision 2:ac98d055a6cd, committed 2015-10-28

Comitter:

dbayuadi

Date:

Wed Oct 28 09:47:23 2015 +0000

Parent:

1:3bae5ab25e20

Commit message:

filter EMG

Changed in this revision

--- a/main.cpp	Wed Oct 21 12:31:55 2015 +0000
+++ b/main.cpp	Wed Oct 28 09:47:23 2015 +0000
@@ -1,55 +1,58 @@
 #include "mbed.h"
 #include "HIDScope.h"
 #include "QEI.h"
-AnalogIn    emg(A0);
-HIDScope scope(5);
+AnalogIn    emg_left(A0);
+AnalogIn emg_right(A1);
+DigitalOut led(LED_BLUE);
+HIDScope scope(6);
 Ticker get;
 AnalogIn pot(A1);
 Serial pc(USBTX, USBRX); // tx, rx
-const int N = 10;
 volatile bool fn_go = false;
 double y_notch_1;
 double y_notch_2;
 double y_high;
 double y_low;
 double y_rect;
-const double Fs = 500;
-const double Ts = 0.002;
+const double Fs = 500*2;
+const double Ts = 1/Fs;
 double a;
-const double gain_notch_1 = 0.912483;
+const double gain_notch_1 = 1.000000;
 const double b0_notch_1 = 1.0*gain_notch_1;
-const double b1_notch_1 = -1.62829098849*gain_notch_1;
+const double b1_notch_1 = -1.90215058158*gain_notch_1;
 const double b2_notch_1 = 1.0*gain_notch_1;
-const double a1_notch_1 = -1.49965530713;
-const double a2_notch_1 =  0.90720693582;
+const double a1_notch_1 = -1.76851639902;
+const double a2_notch_1 =  0.89458928499;
 double v1_notch_1 = 0, v2_notch_1 = 0;
 
 
 const double gain_notch_2 = 1.000000;
 const double b0_notch_2 = 1.0*gain_notch_2;
-const double b1_notch_2 = -1.63220522905*gain_notch_2;
+const double b1_notch_2 = -1.90215058158*gain_notch_2;
 const double b2_notch_2 = 1.0*gain_notch_2;
-const double a1_notch_2 = -1.61200955424;
-const double a2_notch_2 =  0.91813588764;
+const double a1_notch_2 = -1.85196790295;
+const double a2_notch_2 =  0.92089778695;
 double v1_notch_2 = 0, v2_notch_2 = 0;
+//high pass of 5 Hz Wc
 
-const double gain_high = 0.981587;
-const double b0_high = 1.0*gain_high;
-const double b1_high = -1.99999992457*gain_high;
-const double b2_high = 1.0*gain_high;
-const double a1_high = -1.96306054568;
-const double a2_high =  0.96374056522;
+const double gain_high = 1;
+const double b0_high =   0.978030479206560*gain_high;
+const double b1_high = -1.956060958413119*gain_high;
+const double b2_high = 0.978030479206560*gain_high;
+const double a1_high =  -1.955578240315036;
+const double a2_high =   0.956543676511203;
 double v1_high = 0, v2_high = 0;
+double v1_high_1 = 0, v2_high_1=0;
 
-const double gain_low = 0.000561;
-const double b0_low = 1.0*gain_low;
-const double b1_low = 2.000*gain_low;
-const double b2_low = 1.0*gain_low;
-const double a1_low =   -1.93191036285;
-const double a2_low =   0.93415335760;
+//low pass of 0.3 Hz Wc
+const double gain_low = 0.00001;
+const double b0_low =   0.088708177364838*gain_low;
+const double b1_low =  0.177416354729676*gain_low;
+const double b2_low = 0.088708177364838*gain_low;
+const double a1_low =  -1.997334271812535;
+const double a2_low =    0.997337820139629;
 double v1_low = 0, v2_low = 0;
-
-
+double v1_low_1 = 0,v2_low_1=0;
 double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
 {
     double v = u - a1*v1 - a2*v2;
@@ -59,58 +62,135 @@
     return y;
 }
 
+double biquad1(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+double biquad2(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+double biquad3(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+double biquad4(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+}
+
+
+
+
 void fn_activate()
 {
 
     fn_go = true;
 }
 
-double sliding_average(double u,const int f_N)
+double z1;
+double y_high_1;
+double y_notch_1_1;
+double y_notch_2_1;
+double y_rect_1;
+double y_low_1;
+double t = 0;
+double value = 0;
+double max() // calibration
 {
-    double f_x[f_N];
-    double f_sum=0;
-    f_x[1]=abs(u);
-    for (int i=f_N; i>=1; i--){
-        f_x[i]=f_x[i-1];
+    if (t <= 10) {
+        value = value + 0.0;
+    } else if(t>10 && t<=20) {
+        if(value<=y_low) {
+            value = y_low;
+
+        } else {
+            value = value +0.0;
         }
-        
-      
-    for (int i=f_N; i>=0; i--){
-        f_sum=f_x[i]+f_sum;
-       }
-        a=f_sum/double(f_N);
-        f_sum=0;
-    return a;
+
+    } else {
+        value = value + 0.0;
+
+
+    }
+    return value*0.80;
 }
-double z1;
+double val;
+double gain()
+{
+
+    if(max()==0) {
+        val = 0;
+    } else if(max()>=0) {
+        val = 1/max();
+    }
+    return val;
+}
+const double a1 = 11.1111;
+const double b1 = -0.1111;
+const double a2 = 4.7619;
+const double b2 = -0.047619;
 void scopeSend()
 {
-  y_high = biquad(emg.read(), v1_high, v1_high, a1_high, a2_high, b0_high, b1_high, b2_high); 
-  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);
-    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);  
-  y_rect = fabs(y_notch_2);             
-            y_low = biquad(y_rect,  v1_low, v1_low, a1_low, a2_low, b0_low, b1_low, b2_low);
-// z1 = 6*sliding_average(emg.read(), N); //moving average
-scope.set(0,emg.read());
-scope.set(1,y_high);
-  scope.set(2,y_rect);
-  scope.set(3,y_low);
- // scope.set(4,z1);
+    y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high);
+    y_rect = abs(y_high);
+    y_low = biquad2(y_rect,  v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low);
+    t = t+Ts;
+    y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high);
+     y_rect_1 = abs(y_high_1);
+     y_low_1 =   biquad4(y_rect_1,  v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low);
+
+    scope.set(0,emg_left.read());
+    scope.set(1,emg_right.read());
+    scope.set(2,y_low);
+    scope.set(3,y_low_1);
+    scope.set(4,a1*y_low + b1);
+    scope.set(5,a2*y_low_1 + b2);
+
 
     scope.send();
 }
 
 int main()
 {
-get.attach(&fn_activate,Ts);
+    get.attach(&fn_activate,Ts);
 
     while (true) {
-        
+        if(t<=10) {
+            led = 0;
+        }
+        if(t>10 && t<=20)
+        {
+            led = 1;
+        }
+        if(t>20)
+        {
+         led = 0;   
+        }
         if(fn_go == true) {
-            
+
             scopeSend();
-            
-           
+
+
             fn_go = false;
         }
     }