File content as of revision 24:6cde78e776b6:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>

//ATTENTION:   set mBed to version 151
//          set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
//          set MODSERIAL to version 44
//          set HIDScope to version 7
//          set biquadFilter to version 7

AnalogIn emg0_in            (A0);                   //First raw EMG signal input
AnalogIn emg1_in            (A1);                   //Second raw EMG signal input
AnalogIn emg2_in            (A2);                   //Third raw EMG signal input

InterruptIn button1         (D10);                  //Let op, is deze niet bezet? En 11? Even checken, als er een error komt, kan het hier zitten.
InterruptIn button2         (D11);                  //Buttons for switch calibration states

DigitalOut ledr             (LED_RED);              //LEDs to show in which state you are in
DigitalOut ledb             (LED_BLUE);
DigitalOut ledg             (LED_GREEN);

MODSERIAL pc(USBTX, USBRX);                         //Serial communication to see if the code works step by step

HIDScope    scope( 6 );                             //HIDScope set to 3x2 channels for 3 muscles

//HIDscope
Ticker      HIDScope_tick;                          //Ticker for HIDScope
Ticker      filter_tick;                            //Ticker for EMG filter
Ticker      MovAg_tick;                             //Ticker to calculate Moving Average

//Global variables
int encoder     = 0;                                //Starting point encoder (wordt nu nog niet gebruikt in de code)
const float T   = 0.002f;                           //Ticker period

//EMG filter
double emgfilt0, emgfilt1, emgfilt2;                                                       //Variables for filtered EMG data channel 0, 1 and 2
const int windowsize = 150;                                                                      //Size of the array over which the moving average (MovAg) is calculated. (random number)
double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];                //Empty arrays to calculate MoveAg
double movAg0,movAg1,movAg2;                                                                     //outcome of MovAg (moet dit een array zijn??)


//Biquad                                                                                         //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
BiQuadChain emg0filter;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter biquad coefficients

BiQuadChain emg1filter;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter

BiQuadChain emg2filter;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter

//Functions

void HIDScope_sample()
{
    /* 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_raw);
    scope.set(1,emg0_filt);
    scope.set(2,emg1_raw);
    scope.set(3,emg1_filt);
    scope.set(4,emg2_raw);
    scope.set(5,emg2_filt);
    
        //scope.set(0, movAg0.read() ); Werkt niet!! Hoe dan wel moving average in HIDScope??
    
    scope.send();                   //Send data to HIDScope server
}

void EMGFilter0()
{   
    double emg0_raw      = emg0_in.read();                  //give name to raw EMG0 data
    double emg0filt      = emg0filter.step(emg0);           //Use biquad chain to filter raw EMG data
    double emg0filt      = abs(emg0filt);                   //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
}

void EMGFilter1()
{
    double emg1_raw      = emg1_in.read();                  //give name to raw EMG1 data
    double emg1filt      = emg1filter.step(emg1);           //Use biquad chain to filter raw EMG data
    double emg1filt      = abs(emg1filt);                   //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
}

void EMGFilter2()
{
    double emg2_raw      = emg2_in.read();                  //give name to raw EMG1 data
    double emg2filt      = emg2filter.step(emg2);           //Use biquad chain to filter raw EMG data
    double emg2filt      = abs(emg2filt);                   //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
}
 
void MovAg()                                         //Calculate moving average (MovAg)
{
    for (int i = windowsize-1; i>=0; i--)            //Make array of the last datapoints of the filtered signal
    {
        StoreArray0[i] = StoreArray0[i-1];
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
    }
    
    StoreArray0[0] = emgfilt0;                    //Stores the latest datapoint in the first element of the array
    StoreArray1[0] = emgfilt1;
    StoreArray2[0] = emgfilt2;
    
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;
    
    for(int a = 0; a<= windowsize-1; a++)            //Sum the elements in the array
    {
        sum1 += StoreArray0[a];
        sum2 += StoreArray1[a];
        sum3 += StoreArray2[a];
    }
    
    movAg0 = sum1/windowsize;                        //calculates an average in the array
    movAg1 = sum2/windowsize;
    movAg2 = sum3/windowsize;
    //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
}

void emg_filtered()             //Call all filter functions
{
    EMGFilter0();
    EMGFilter1();
    EMGFilter2();
    MovAg();
}



int main()
{         
        pc.baud(115200);
        pc.printf("hello\n\r");                                                                 //Check, does it work?
        
        emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 );        //attach biquad elements to chain
        emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
        emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );

        while(true)
        {

        filter_tick.attach(&emg_filtered,T);        //EMG signals filtered + moving average every T sec.
        HIDScope_tick.attach(&HIDScope_sample,T);   //EMG signals raw + filtered to HIDScope every T sec.

        pc.printf("\n\r Moving average EMG 3 is: %d \n\r",movAg2);
        
        }
}