File content as of revision 22:611667172ac3:

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

//Define objects
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
//AnalogIn    emg2( A2 );
//AnalogIn    emg3( A3 );
float A;
float B;
Ticker      sample_timer;
HIDScope    scope( 5 );
DigitalOut  led(LED1);

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;

   
void sample()
{    
    
    float A = emg0.read();
    
    for (int j=leng_filt-1; j>=1; j--)
    {
        A_array[j] = A_array[j-1];
    }
    
    A_array[0] = A;
    Asum = 0;
    
    for(int i=0; i<=leng_filt-1; i++)
    {
        Asum += A_array[i]*1/leng_filt;
    }
    
    // Vanaf hier begint het butterworth laagdoorlaatfilter    
    
    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;
    
    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 Amean2 = 0;
   
    for (int j=leng_filt-1; j>=1; j--)
    {
        Asum_ar[j] = Asum_ar[j-1];
    }

    Asum_ar[0] = Asum;
    
    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);       
    
    // 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 ))
    *  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*/
    while(1) {}
}