File content as of revision 25:cfd422c3cf4d:

#include "mbed.h"
#include "HIDScope.h"
#include "MAF.h"
#include "vector"
#include "numeric"
#include "BiQuad.h"


//Define objects
AnalogIn    emg0( A0 ); // 1e
AnalogIn    emg2( A2 ); // 2e
AnalogIn    emg4( A4 ); // 3e

float A;
float B;
Ticker      sample_timer;
HIDScope    scope( 3 );
DigitalOut  led(LED1);

const int leng_filt = 60;
const int box_length = 50;
const int box_lengthC = 150;
const int box_checkC = 50;
const float grenswaardeA0 = 0.025;
const float grenswaardeB0 = 0.015;
//const float grenswaardeA1 = 0.022;
//const float grenswaardeB1 = 0.018;
const float grenswaardeC = 0.015;
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 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_lengthC] = {0};

float result = 0;
float Asum = 0;
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.00000;
const double a2 = 0.171573;
   
void sample()
{    
    BiQuad lowpassA(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_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 = lowpassA.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>grenswaardeA0)
    {   Ay3 = 1; }
    
    //if(Ay2<=grenswaardeA0)
    //{   if(Ay2>grenswaardeA1)
    //    {   Ay3 = 0.5; }   }
    
    if(Ay2<=grenswaardeA0)
    {   Ay3 = 0; }
    
    for (int j=box_length-1; j>=1; j--)
    {   A_ar3[j] = A_ar3[j-1];         }
    
    A_ar3[0] = Ay3;
    int boxcheckA = 0;
    
    for (int j=0; j<=box_length-1; j++)
    {   if(A_ar3[j] == 1)
            { boxcheckA = 1;}           }

    //for (int j=0; j<=box_length-1; j++)
    //    {   if(A_ar3[j] == 0.5)
    //        { boxcheckA = 0.5;}           }

    // Signaal 2 (B)    
    BiQuad lowpassB(b0,b1, b2, a0, a1, a2);
    float B = emg2.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 = lowpassB.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++)
    {   By2 += B_ar2[i]*1/leng_filt;     }
    
    float By3;
    if(By2>grenswaardeB0)
    {   By3 = 1; }
    
    //if(By2<=grenswaardeB0)
    //{   if(By2>grenswaardeB1)
    //    {   By3 = 0.5; }   }
    
    if(By2<=grenswaardeB0)
    {   By3 = 0; }
    
    for (int j=box_length-1; j>=1; j--)
    {   B_ar3[j] = B_ar3[j-1];         }
    
    B_ar3[0] = By3;
    int boxcheckB = 0;
    
    for (int j=0; j<=box_length-1; j++)
    {   if(B_ar3[j] == 1)
            { boxcheckB = 1;}           }
    
    //for (int j=0; j<=box_length-1; j++)
    //{   if(B_ar3[j] == 0.5)
    //        { boxcheckB = 0.5;}           }
    
    
    // Signaal 3 (C)
    BiQuad lowpassC(b0,b1, b2, a0, a1, a2);
    float C = emg4.read();
    float Cmean = 0;
    float Cy2 = 0;
    
    for (int j=leng_filt-1; j>=1; j--)
    {    C_ar[j] = C_ar[j-1];           }

    C_ar[0] = C;
    
    for(int i=0; i<=leng_filt-1; i++)
    {    Cmean += C_ar[i]*1/leng_filt;  }
    
    Cy1 = C - Cmean;
    Cy1 = fabs(Cy1);
    Cy1 = lowpassC.step(Cy1);              // First signal, after Butterworth  
    
    for (int j=leng_filt-1; j>=1; j--)
    {   C_ar2[j] = C_ar2[j-1];          }
    
    C_ar2[0] = Cy1;
    
    for(int i=0; i<=leng_filt-1; i++)
    {   Cy2 += C_ar2[i]*1/leng_filt;    }
    
    float Cy3;
    if(Cy2>grenswaardeC)
    {   Cy3 = 1; }
    
    if(Cy2<=grenswaardeC)
    {   Cy3 = 0; }
    
    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, boxcheckA);
    scope.set(1, boxcheckB);
    scope.set(2, boxcheckC);
    
    scope.send();
    led = !led;
}

int main()
{   
    sample_timer.attach(&sample, 0.005);
    while(1) {}
}