Diff: main.cpp

Revision:

10:c7140e085ff3

Parent:

9:cc60d28643cb

--- a/main.cpp	Mon Oct 20 10:58:07 2014 +0000
+++ b/main.cpp	Mon Oct 20 11:27:55 2014 +0000
@@ -3,15 +3,15 @@
 #include "HIDScope.h"
 #include <iostream>
 
-#define A1LP1    0.018180963222803
-#define A0LP1     0.016544013176248
-#define B1LP1    -1.718913340044714
-#define B0LP1     0.753638316443765
+#define A1LP    0.018180963222803
+#define A0LP     0.016544013176248
+#define B1LP    -1.718913340044714
+#define B0LP     0.753638316443765
 
-#define A1HP1  -1.999801878951505
-#define A0HP1   0.999801878951505
-#define B1HP1   -1.971717601075000
-#define B0HP1  0.972111984032897
+#define A1HP    -1.999801878951505
+#define A0HP  0.999801878951505
+#define B1HP   -1.971717601075000
+#define B0HP  0.972111984032897
 
 #define A0N 0.99436777112
 #define A1N -1.89139989664
@@ -21,40 +21,41 @@
 
 #define TSAMP 0.001
 
-AnalogIn    emg0(PTB1);
+AnalogIn    emg1(PTB1);
+AnalogIn    emg2(PTB0);
 MODSERIAL pc(USBTX,USBRX);
-HIDScope scope(5);
+HIDScope scope(6);
 Ticker timer;
-float emg_value, ylp1, yhp1, yn;
+float emg_value1, ylp1, yhp1, yn1;
+float emg_value2, ylp2, yhp2, yn2;
 volatile bool looptimerflag;
-float ysum = 0, yave=0 ;
-
+float ysum1 = 0, yave1=0, ysum2 =0, yave2=0 ;
 
-float readEMG()
+//filters signaal 1
+float readEMG1()
 {
-
-    emg_value=emg0.read();
-    return emg_value;
+    emg_value1=emg1.read();
+    return emg_value1;
 }
 
-float notchfilter(float ylp1)
+float notchfilter1(float emg_value1)
 {
-    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;
+    static float yn1,x1=0,x2=0,y1=0,y2=0,x;
+    x = emg_value1;
+    yn1 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
     x2 = x1;
     x1 = x;
     y2 = y1;
-    y1 = yn;
-    return yn;
+    y1 = yn1;
+    return yn1;
 }
 
 
-float hpfilter1(float yn)
+float hpfilter1(float yn1)
 {
     static float x1=0,y1=0,x2=0, y2=0,x;
-    x = yn;
-    yhp1 = x + A1HP1*x1 + A0HP1*x2 - B1HP1*y1 - B0HP1*y2;
+    x = yn1;
+    yhp1 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
     x2 = x1;
     x1 = x;
     y2 = y1;
@@ -66,23 +67,67 @@
 {
     static float x1=0,y1=0,x2=0, y2=0,x;
     x = yhp1;
-    ylp1 = A1LP1*x1-B1LP1*y1+A0LP1*x2-B0LP1*y2;
+    ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
     x2 = x1;
     x1 = x;
     y2 = y1;
     y1 = ylp1;
     return ylp1;
 }
+//filters emg singaal 2
+float readEMG2()
+{
+    emg_value2=emg2.read();
+    return emg_value2;
+}
+
+float notchfilter2(float emg_value2)
+{
+    static float yn2,x1=0,x2=0,y1=0,y2=0,x;
+    x = emg_value2;
+    yn2 = A0N*x + A1N*x1+A2N*x2+B1N*y1+B2N*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yn2;
+    return yn2;
+}
+
+
+float hpfilter2(float yn2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yn2;
+    yhp2 = x + A1HP*x1 + A0HP*x2 - B1HP*y1 - B0HP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = yhp2;
+    return yhp2;
+}
+
+float lpfilter2(float yhp2)
+{
+    static float x1=0,y1=0,x2=0, y2=0,x;
+    x = yhp2;
+    ylp1 = A1LP*x1-B1LP*y1+A0LP*x2-B0LP*y2;
+    x2 = x1;
+    x1 = x;
+    y2 = y1;
+    y1 = ylp2;
+    return ylp2;
+}
 
 
 void viewer()
 {
-    scope.set(0,emg_value);
-    scope.set(1,yn);
-    scope.set(2,yhp1);
-    scope.set(3,ylp1);
-    scope.set(4,yave);
-    
+    scope.set(0,emg_value1);
+    scope.set(1,ylp1);
+    scope.set(2,yave1);
+    scope.set(3,emg_value2);
+    scope.set(4,ylp2);
+    scope.set(5,yave2);
+
     scope.send();
 }
 
@@ -95,27 +140,45 @@
 {
     pc.baud(115200);
     timer.attach(setlooptimerflag,TSAMP);
-    int n = 0;
+    int n = 0; //n and m are used for the loops underneath
+    int m = 0;
     while(1) {
 
         while(!looptimerflag) /* do nothing */;
         looptimerflag = false;
-        emg_value =  readEMG();
-        yn = notchfilter(emg_value);
-        yhp1 = hpfilter1(yn);  //function hpfilter
+
+        emg_value1 = readEMG1();
+        emg_value2 = readEMG2();
+
+        yn1 = notchfilter1(emg_value1);
+        yn2 = notchfilter2(emg_value2);
+
+        yhp1 = hpfilter1(yn1);   //function hpfilter
+        yhp2 = hpfilter2(yn2);
+
         ylp1 = lpfilter1(yhp1);       //function filter
-       
-        ysum = ysum+sqrt(ylp1*ylp1);
+        ylp2 = lpfilter2(yhp2);
+
+        ysum1 = ysum1+sqrt(ylp1*ylp1);
+        ysum2 = ysum2+sqrt(ylp2*ylp2);
+
         n++;
-       
+        m++;
         if(n==100) {
-            yave = abs(ysum/100);
-            ysum = 0;
+            yave1 = ysum1/100;
+            ysum1 = 0;
             n = 0;
-            }
+        }
+        if(m==1000){
+            yave2 = ysum2/100;
+            ysum2 = 0;
+            m=0;
+        }
 
-         viewer();                  //functie hidscope
-        
+            
+
+        viewer();                  //function hidscope
+
 
     }
 }
\ No newline at end of file