Diff: main.cpp

Revision:

22:611667172ac3

Parent:

21:f6e70856810c

Child:

23:a6f18aee31cd

diff -r f6e70856810c -r 611667172ac3 main.cpp
--- a/main.cpp	Tue Oct 29 14:48:30 2019 +0000
+++ b/main.cpp	Wed Oct 30 08:28:29 2019 +0000
@@ -13,14 +13,16 @@
 float A;
 float B;
 Ticker      sample_timer;
-HIDScope    scope( 3 );
+HIDScope    scope( 5 );
 DigitalOut  led(LED1);
 
-const int leng_filt = 10;
+const int leng_filt = 20;
 float A_array[leng_filt] = {0};
 float B_array[leng_filt] = {0};
 float Asum_ar[leng_filt] = {0};
 float Bsum_ar[leng_filt] = {0};
+float Asum_ar2[leng_filt] = {0};
+float Bsum_ar2[leng_filt] = {0};
 float result = 0;
 float Asum = 0;
 float Bsum = 0;
@@ -30,70 +32,83 @@
 {    
     
     float A = emg0.read();
-    float B = emg1.read();
     
     for (int j=leng_filt-1; j>=1; j--)
     {
         A_array[j] = A_array[j-1];
-        B_array[j] = B_array[j-1];
     }
     
     A_array[0] = A;
-    B_array[0] = B;
-    
     Asum = 0;
-    Bsum = 0;
     
     for(int i=0; i<=leng_filt-1; i++)
     {
         Asum += A_array[i]*1/leng_filt;
-        Bsum += B_array[i]*1/leng_filt;
     }
     
-    
     // Vanaf hier begint het butterworth laagdoorlaatfilter    
     
     const int Fs = 2000; //Sample Frequency
-    const double b0 = 0.097631;
-    const double b1 = 0.195262;
-    const double b2 = 0.097631;
+    const double b0 = 0.292893;
+    const double b1 = 0.585786;
+    const double b2 = 0.292893;
     const double a0 = 1.000000;
-    const double a1 = -0.942809;
-    const double a2 = 0.333333;
+    const double a1 = -0;
+    const double a2 = 0.171573;
     
     BiQuad lowpass(b0,b1, b2, a0, a1, a2);
 
     // voor bepaald ingangssignaal u1 en output y1
+    // Eerst mean dan Buttterworth
+    
     double u1 = Asum;
     double y1;
     
     float Amean = 0;
-    float Bmean = 0;
-    
+    float Amean2 = 0;
+   
     for (int j=leng_filt-1; j>=1; j--)
     {
         Asum_ar[j] = Asum_ar[j-1];
-        Bsum_ar[j] = Bsum_ar[j-1];
     }
-    
+
     Asum_ar[0] = Asum;
-    Bsum_ar[0] = Bsum;
     
     for(int i=0; i<=leng_filt-1; i++)
     {
         Amean += Asum_ar[i]*1/leng_filt;
-        Bmean += Bsum_ar[i]*1/leng_filt;
     }
     
     y1 = u1 - Amean; //offset?
     y1 = fabs(y1);
     y1 = lowpass.step(y1);       
-        
+    
+    // Eerst butterworth dan mean
+    float y2;
+    y2 = A - Asum;
+    y2 = fabs(y2);
+    y2 = lowpass.step(y2);
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {
+        Asum_ar2[j] = Asum_ar2[j-1];
+    }
+    
+    Asum_ar2[0] = y2;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {
+        Amean2 += Asum_ar2[i]*1/leng_filt;
+    }
+
+    
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     
     scope.set(0, emg0.read());
     scope.set(1, Asum);
     scope.set(2, y1);
+    scope.set(3, y2);
+    scope.set(4, Amean2);
     
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
     *  Ensure that enough channels are available (HIDScope scope( 2 ))