Diff: main.cpp

Revision:

6:6f0bc2e465b0

Parent:

5:9415e5be8235

Child:

7:5f1c0bcd3f69

--- a/main.cpp	Fri Oct 17 10:23:42 2014 +0000
+++ b/main.cpp	Mon Oct 20 09:50:55 2014 +0000
@@ -3,13 +3,21 @@
 #include "HIDScope.h"
 #include <iostream>
 
-#define A1LP    0.221199216928595
-#define B1LP    -0.778800783071405
+#define A1LP1    0.018180963222803
+#define A0LP1     0.016544013176248
+#define B1LP1    -1.718913340044714
+#define B0LP1     0.753638316443765
 
-#define A1HP  -1.999801878951505
-#define A0HP   0.999801878951505
-#define B1HP   -1.971717601075000
-#define B0HP  0.972111984032897
+#define A1HP1  -1.999801878951505
+#define A0HP1   0.999801878951505
+#define B1HP1   -1.971717601075000
+#define B0HP1  0.972111984032897
+
+#define A0N 0.99436777112
+#define A1N -1.89139989664
+#define A2N 0.99436777112
+#define B1N 1.89070035439
+#define B2N -0.988036
 
 #define TSAMP 0.001
 
@@ -17,7 +25,7 @@
 MODSERIAL pc(USBTX,USBRX);
 HIDScope scope(5);
 Ticker timer;
-float emg_value, ylp1, yhp1;
+float emg_value, ylp1, yhp1, yn;
 volatile bool looptimerflag;
 float ysum = 0, yave=0 ;
 
@@ -31,32 +39,48 @@
 
 float hpfilter1(float emg_value)
 {
-    static float y,x1=0,y1=0,x2=0, y2=0;
-    yhp1 = y1*B1HP-y2*B0HP-x1*A1HP+x2*A0HP;
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = emg_value;
+    yhp1 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 + B0HP1*y2;
     x2 = x1;
-    x1 = emg_value;
+    x1 = x;
     y2 = y1;
-    y1 = y;
+    y1 = yhp1;
     return yhp1;
 }
 
 float lpfilter1(float yhp1)
 {
-    static float y,x1=0,y1=0,x=0;
-    ylp1 = A1LP*x1+y1*B1LP;
-    y1 = y;
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yhp1;
+    ylp1 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
+    x2 = x1;
     x1 = x;
-    x = yhp1;
+    y2 = y1;
+    y1 = ylp1;
     return ylp1;
 }
 
+float notchfilter(float ylp1)
+{
+    static float yn,x1=0,x2=0,y1=0,y2=0,x;
+    x = ylp1;
+    yn = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yn;
+    return yn;
+}
+
 void viewer()
 {
     scope.set(0,emg_value);
     scope.set(1,yhp1);
     scope.set(2,ylp1);
-    scope.set(3,yave);
-    scope.set(4,ysum);
+    scope.set(3,yn);
+    scope.set(4,yave);
+    
     scope.send();
 }
 
@@ -76,8 +100,9 @@
         looptimerflag = false;
         emg_value =  readEMG();
         yhp1 = hpfilter1(emg_value);  //function hpfilter
-        ylp1 = lpfilter1(emg_value);       //function filter
-        ysum = ysum+yhp1;
+        ylp1 = lpfilter1(yhp1);       //function filter
+        yn = notchfilter(ylp1);
+        ysum = ysum+yn;
         n++;
        
         if(n==100) {