Diff: main.cpp

Revision:

23:a6f18aee31cd

Parent:

22:611667172ac3

Child:

25:cfd422c3cf4d

diff -r 611667172ac3 -r a6f18aee31cd main.cpp
--- a/main.cpp	Wed Oct 30 08:28:29 2019 +0000
+++ b/main.cpp	Wed Oct 30 12:11:18 2019 +0000
@@ -16,115 +16,190 @@
 HIDScope    scope( 5 );
 DigitalOut  led(LED1);
 
-const int leng_filt = 20;
+const int leng_filt = 60;
+const int box_length = 100;
+const int box_lengthC = 200;
+const int box_checkC = 150;
+const float grenswaardeA = 0.01;
+const float grenswaardeB = 0.02;
+const float grenswaardeC = 0.28;
+
+float Ay1;
+float Ay2;
 float A_array[leng_filt] = {0};
+float A_ar[leng_filt] = {0};
+float A_ar2[leng_filt] = {0};
+float A_ar3[box_length] = {0};
+
+float By1;
+float By2;
 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 B_ar[leng_filt] = {0};
+float B_ar2[leng_filt] = {0};
+float B_ar3[box_length] = {0};
+
+float Cy1;
+float Cy2;
+float C_array[leng_filt] = {0};
+float C_ar[leng_filt] = {0};
+float C_ar2[leng_filt] = {0};
+float C_ar3[box_length] = {0};
+
 float result = 0;
 float Asum = 0;
-float Bsum = 0;
-
+const int Fs = 2000; //Sample Frequency
+const double b0 = 0.206572;
+const double b1 = 0.413144;
+const double b2 = 0.206572;
+const double a0 = 1.000000;
+const double a1 = -0.369527;
+const double a2 = 0.195816;
    
 void sample()
 {    
-    
+    BiQuad lowpass(b0,b1, b2, a0, a1, a2);
+    // Signaal 1 (A)
     float A = emg0.read();
+    float Amean = 0;
+    float Ay2 = 0;
     
     for (int j=leng_filt-1; j>=1; j--)
-    {
-        A_array[j] = A_array[j-1];
-    }
+    {    A_ar[j] = A_ar[j-1];           }
+
+    A_ar[0] = A;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {    Amean += A_ar[i]*1/leng_filt;  }
+    
+    Ay1 = A - Amean;
+    Ay1 = fabs(Ay1);
+    Ay1 = lowpass.step(Ay1);              // First signal, after Butterworth  
+        
+    for (int j=leng_filt-1; j>=1; j--)
+    {   A_ar2[j] = A_ar2[j-1];          }
+    
+    A_ar2[0] = Ay1;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {   Ay2 += A_ar2[i]*1/leng_filt;     }
+    
+    float Ay3;
+    if(Ay2>grenswaardeA)
+    {   Ay3 = 1; }
+    
+    if(Ay2<=grenswaardeA)
+    {   Ay3 = 0; }
+    
+    for (int j=box_length-1; j>=1; j--)
+    {   A_ar3[j] = A_ar3[j-1];         }
     
-    A_array[0] = A;
-    Asum = 0;
+    A_ar3[0] = Ay3;
+    int boxcheckA = 0;
+    
+    for (int j=0; j<=box_length-1; j++)
+    {   if(A_ar3[j] == 1)
+            { boxcheckA = 1;}           }
+
+    // Signaal 2 (B)
+    
+    float B = emg0.read();
+    float Bmean = 0;
+    float By2 = 0;
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {    B_ar[j] = B_ar[j-1];           }
+
+    B_ar[0] = B;
+    
+    for(int i=0; i<=leng_filt-1; i++)
+    {    Bmean += B_ar[i]*1/leng_filt;  }
+    
+    By1 = B - Bmean;
+    By1 = fabs(By1);
+    By1 = lowpass.step(By1);              // First signal, after Butterworth  
+        
+    for (int j=leng_filt-1; j>=1; j--)
+    {   B_ar2[j] = B_ar2[j-1];          }
+    
+    B_ar2[0] = By1;
     
     for(int i=0; i<=leng_filt-1; i++)
-    {
-        Asum += A_array[i]*1/leng_filt;
-    }
+    {   By2 += B_ar2[i]*1/leng_filt;     }
+    
+    float By3;
+    if(By2>grenswaardeB)
+    {   By3 = 1; }
     
-    // Vanaf hier begint het butterworth laagdoorlaatfilter    
+    if(By2<=grenswaardeB)
+    {   By3 = 0; }
+    
+    for (int j=box_length-1; j>=1; j--)
+    {   B_ar3[j] = B_ar3[j-1];         }
     
-    const int Fs = 2000; //Sample Frequency
-    const double b0 = 0.292893;
-    const double b1 = 0.585786;
-    const double b2 = 0.292893;
-    const double a0 = 1.000000;
-    const double a1 = -0;
-    const double a2 = 0.171573;
+    B_ar3[0] = By3;
+    int boxcheckB = 0;
+    
+    for (int j=0; j<=box_length-1; j++)
+    {   if(B_ar3[j] == 1)
+            { boxcheckB = 1;}           }
     
-    BiQuad lowpass(b0,b1, b2, a0, a1, a2);
+    // Signaal 3 (C)
+    
+    float C = emg0.read();
+    float Cmean = 0;
+    float Cy2 = 0;
+    
+    for (int j=leng_filt-1; j>=1; j--)
+    {    C_ar[j] = C_ar[j-1];           }
 
-    // voor bepaald ingangssignaal u1 en output y1
-    // Eerst mean dan Buttterworth
-    
-    double u1 = Asum;
-    double y1;
-    
-    float Amean = 0;
-    float Amean2 = 0;
-   
-    for (int j=leng_filt-1; j>=1; j--)
-    {
-        Asum_ar[j] = Asum_ar[j-1];
-    }
-
-    Asum_ar[0] = Asum;
+    C_ar[0] = C;
     
     for(int i=0; i<=leng_filt-1; i++)
-    {
-        Amean += Asum_ar[i]*1/leng_filt;
-    }
-    
-    y1 = u1 - Amean; //offset?
-    y1 = fabs(y1);
-    y1 = lowpass.step(y1);       
+    {    Cmean += C_ar[i]*1/leng_filt;  }
     
-    // Eerst butterworth dan mean
-    float y2;
-    y2 = A - Asum;
-    y2 = fabs(y2);
-    y2 = lowpass.step(y2);
+    Cy1 = C - Cmean;
+    Cy1 = fabs(Cy1);
+    Cy1 = lowpass.step(Cy1);              // First signal, after Butterworth  
+        
+    for (int j=leng_filt-1; j>=1; j--)
+    {   C_ar2[j] = C_ar2[j-1];          }
     
-    for (int j=leng_filt-1; j>=1; j--)
-    {
-        Asum_ar2[j] = Asum_ar2[j-1];
-    }
-    
-    Asum_ar2[0] = y2;
+    C_ar2[0] = Cy1;
     
     for(int i=0; i<=leng_filt-1; i++)
-    {
-        Amean2 += Asum_ar2[i]*1/leng_filt;
-    }
-
+    {   Cy2 += C_ar2[i]*1/leng_filt;     }
+    
+    float Cy3;
+    if(Cy2>grenswaardeC)
+    {   Cy3 = 1; }
+    
+    if(Cy2<=grenswaardeC)
+    {   Cy3 = 0; }
     
-    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
+    for (int j=box_lengthC-1; j>=1; j--)
+    {   C_ar3[j] = C_ar3[j-1];         }
     
+    C_ar3[0] = Cy3;
+    int boxcheckC = 0;
+    int C_sum = 0;
+    for (int j=0; j<=box_length-1; j++)
+    {   C_sum += C_ar3[j];
+        if(C_sum >= box_checkC)
+        {   boxcheckC = 1;}  }
+        
+        
     scope.set(0, emg0.read());
-    scope.set(1, Asum);
-    scope.set(2, y1);
-    scope.set(3, y2);
-    scope.set(4, Amean2);
+    scope.set(1, Cy1);
+    scope.set(2, Cy2);
+    scope.set(3, Cy3);
+    scope.set(4, boxcheckC);
     
-    /* 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 ))
-    *  Finally, send all channels to the PC at once */
     scope.send();
-    /* To indicate that the function is working, the LED is toggled */
     led = !led;
 }
 
 int main()
 {   
-    /**Attach the 'sample' function to the timer 'sample_timer'.
-    * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
-    */
-    sample_timer.attach(&sample, 0.002);
-
-    /*empty loop, sample() is executed periodically*/
+    sample_timer.attach(&sample, 0.001);
     while(1) {}
 }
\ No newline at end of file