Diff: main.cpp

Revision:

12:8604a4c1c708

Parent:

11:91d827c62a50

Child:

13:23130c3a37b1

--- a/main.cpp	Wed Oct 22 09:24:13 2014 +0000
+++ b/main.cpp	Wed Oct 22 10:42:46 2014 +0000
@@ -22,13 +22,15 @@
 #define TSAMP 0.001
 
 AnalogIn    emg1(PTB1);
+AnalogIn    emg2(PTB0);
 MODSERIAL pc(USBTX,USBRX);
 HIDScope scope(5);
 Ticker timer;
+float emg_value2, ylp2, yhp2, yn2;
 float emg_value1, ylp1, yhp1, yn1;
 volatile bool looptimerflag;
 float ysum1 = 0, yave1=0 ;
-
+float ysum2 = 0, yave2=0 ;
 
 float readEMG()
 {
@@ -37,6 +39,26 @@
     return emg_value1;
 }
 
+
+float readEMG2()
+{
+
+    emg_value2=emg2.read();
+    return emg_value2;
+}
+
+float notchfilter2(float ylp2)
+{
+    static float x1=0,x2=0,y1=0,y2=0,x;
+    x = ylp2;
+    yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yn2;
+    return yn2;
+}
+
 float notchfilter1(float ylp1)
 {
     static float yn1,x1=0,x2=0,y1=0,y2=0,x;
@@ -50,6 +72,17 @@
 }
 
 
+float hpfilter2(float yn2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yn2;
+    yhp2 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yhp2;
+    return yhp2;
+}
 float hpfilter1(float yn1)
 {
     static float x1=0,y1=0,x2=0, y2=0,x;
@@ -62,6 +95,18 @@
     return yhp1;
 }
 
+float lpfilter2(float yhp2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yhp2;
+    ylp2 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = ylp2;
+    return ylp2;
+}
+
 float lpfilter1(float yhp1)
 {
     static float x1=0,y1=0,x2=0, y2=0,x;
@@ -75,13 +120,15 @@
 }
 
 
+
 void viewer()
 {
     scope.set(0,emg_value1);
-    scope.set(1,yn1);
-    scope.set(2,yhp1);
-    scope.set(3,ylp1);
-    scope.set(4,yave1);
+    scope.set(1,ylp1);
+    scope.set(2,yave1);
+    scope.set(3,ylp2);
+    scope.set(4,yave2);
+ 
     
     scope.send();
 }
@@ -96,6 +143,7 @@
     pc.baud(115200);
     timer.attach(setlooptimerflag,TSAMP);
     int n = 0;
+    int m = 0;
     while(1) {
 
         while(!looptimerflag) /* do nothing */;
@@ -107,13 +155,26 @@
        
         ysum1 = ysum1+sqrt(ylp1*ylp1);
         n++;
-       
         if(n==100) {
             yave1 = abs(ysum1/100);
             ysum1 = 0;
             n = 0;
             }
 
+        emg_value2 =  readEMG2();
+        yn2 = notchfilter2(emg_value2);
+        yhp2 = hpfilter2(yn2);  //function hpfilter
+        ylp2 = lpfilter2(yhp2);       //function filter
+       
+        ysum2 = ysum2 +sqrt(ylp2*ylp2);
+        m++;
+       
+        if(m==1000) {
+            yave2 = abs(ysum2/1000);
+            ysum2 = 0;
+            m = 0;
+            }
+
          viewer();                  //functie hidscope