File content as of revision 26:874d50f440d0:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"

#include <iostream>
#include "MovingAverage.h"
#define NSAMPLE 200
DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);


//MovingAverage
MovingAverage <float>Movag_1(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
MovingAverage <float>Movag_2(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
MovingAverage <float>Movag_3(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
MovingAverage <float>Movag_4(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above

//EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
Ticker sample_ticker; //ticker for filtering pref. with 1000Hz, define in tick.attach
Ticker threshold_check_ticker;
Timer t; //timer try out for Astrid
Timer timer_calibration; //timer for EMG calibration

//Input
AnalogIn    emg1( A1 );
AnalogIn    emg2( A2 );
AnalogIn    emg3( A3 );
AnalogIn    emg4( A4 );

// GLOBALS EMG
//Filtered EMG signals from the end of the chains
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
int i = 0;

//Define doubles for calibration and ticker
double ts = 0.001; //tijdsstap
double calibration_time = 55; //time EMG calibration should take

volatile double temp_highest_emg1 = 0; //highest detected value right biceps
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;

//Doubles for calculation threshold
double duim_p_t = 0.5; //set threshold at percentage of highest value
double bicep_p_t = 0.5; //
double tricep_p_t = 0.5; //set threshold at percentage of highest value
double onderarm_p_t = 0.5;

volatile double threshold1;
volatile double threshold2;
volatile double threshold3;
volatile double threshold4;

// thresholdreads bools
int Duim;
int Bicep;
int Tricep;
int Onderarm;

//  EMG OUTPUT
int EMGxplus;
int EMGxmin ;
int EMGyplus;
int EMGymin ;


//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass 
BiQuadChain highp1;
BiQuad highp1_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
BiQuad highp1_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );

//Notch
BiQuadChain notch1;
BiQuad notch1_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
BiQuad notch1_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );

//Lowpass 4th order cutoff 3Hz
BiQuadChain lowp1;
BiQuad lowp1_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
BiQuad lowp1_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );

//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass
BiQuadChain highp2;
BiQuad highp2_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
BiQuad highp2_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );

//Notch
BiQuadChain notch2;
BiQuad notch2_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
BiQuad notch2_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );

//Lowpass 4th order cutoff 3Hz
BiQuadChain lowp2;
BiQuad lowp2_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
BiQuad lowp2_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );

//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass
BiQuadChain highp3;
BiQuad highp3_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
BiQuad highp3_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );

//Notch
BiQuadChain notch3;
BiQuad notch3_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
BiQuad notch3_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );

//Lowpass 4th order cutoff 3Hz
BiQuadChain lowp3;
BiQuad lowp3_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
BiQuad lowp3_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );

//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass
BiQuadChain highp4;
BiQuad highp4_1( 9.21171e-01, -1.84234e+00, 9.21171e-01, -1.88661e+00, 8.90340e-01 );
BiQuad highp4_2( 1.00000e+00, -2.00000e+00, 1.00000e+00, -1.94922e+00, 9.53070e-01 );

//Notch
BiQuadChain notch4;
BiQuad notch4_1( 9.56543e-01, -1.82035e+00, 9.56543e-01, -1.84459e+00, 9.53626e-01 );
BiQuad notch4_2( 1.00000e+00, -1.90305e+00, 1.00000e+00, -1.87702e+00, 9.59471e-01 );

//Lowpass 4th order cutoff 3Hz
BiQuadChain lowp4;
BiQuad lowp4_1( 7.69910e-09, 1.53982e-08, 7.69910e-09, -1.96542e+00, 9.65769e-01 );
BiQuad lowp4_2( 1.00000e+00, 2.00000e+00, 1.00000e+00, -1.98532e+00, 9.85677e-01 );

// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
void emgsample()
{
    
    //All EMG signal through Highpass
    double emgread1 = emg1.read();
    double emgread2 = emg2.read();
    double emgread3 = emg3.read();
    double emgread4 = emg4.read();

    double emg1_highpassed = highp1.step(emgread1);
    double emg2_highpassed = highp2.step(emgread2);
    double emg3_highpassed = highp3.step(emgread3);
    double emg4_highpassed = highp4.step(emgread4);

    //All EMG highpassed through Notch
    double emg1_notched = notch1.step(emg1_highpassed);
    double emg2_notched = notch2.step(emg2_highpassed);
    double emg3_notched = notch3.step(emg3_highpassed);
    double emg4_notched = notch4.step(emg4_highpassed);

    //All EMG notched rectify
    double emg1_abs = abs(emg1_notched);
    double emg2_abs = abs(emg2_notched);
    double emg3_abs = abs(emg3_notched);
    double emg4_abs = abs(emg4_notched);

    //All EMG abs into lowpass
    double emg1_lp = lowp1.step(emg1_abs);
    double emg2_lp = lowp2.step(emg2_abs);
    double emg3_lp = lowp3.step(emg3_abs);
    double emg4_lp = lowp4.step(emg4_abs);
    
    Movag_1.Insert(emg1_lp);
    Movag_2.Insert(emg2_lp);
    Movag_3.Insert(emg3_lp);
    Movag_4.Insert(emg4_lp);
    
    emg1_filtered = Movag_1.GetAverage();
    emg2_filtered = Movag_2.GetAverage();
    emg3_filtered = Movag_3.GetAverage();
    emg4_filtered = Movag_4.GetAverage();
    
    

}

void CalibrationEMG()
{
    //static float samples = calibration_time/ts;
    while(timer_calibration<55) {
        if(timer_calibration>0 && timer_calibration<10) {
            led1=!led1;
            if(emg1_filtered>temp_highest_emg1) {
                temp_highest_emg1= emg1_filtered;
                
            }
        }
        if(timer_calibration>10 && timer_calibration<15) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>15 && timer_calibration<25) {
            led2=!led2;
            if(emg2_filtered>temp_highest_emg2) {
                temp_highest_emg2= emg2_filtered;
                
            }
        }
        if(timer_calibration>25 && timer_calibration<30) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>30 && timer_calibration<40) {
            led3=!led3;
            if(emg3_filtered>temp_highest_emg3) {
                temp_highest_emg3= emg3_filtered;
                
            }
        }
        if(timer_calibration>40 && timer_calibration<45) {
            led1=0;
            led2=0;
            led3=0;
        }
        if(timer_calibration>45 && timer_calibration<55) {
            led2=!led2;
            led3=!led3;
            if(emg4_filtered>temp_highest_emg4) {
                temp_highest_emg4= emg4_filtered;
                
            }
        }
        led1=1;
        led2=1;
        led3=1;


    }

    threshold1 = temp_highest_emg1*duim_p_t;  //Right Biceps
    threshold2 = temp_highest_emg2*bicep_p_t; //Right Triceps
    threshold3 = temp_highest_emg3*tricep_p_t;  //Left Biceps
    threshold4 = temp_highest_emg4*onderarm_p_t; //Left Triceps
}

//Check if emg_filtered has reached their threshold
void threshold_check()
{

    //EMG1 threshold check
    if(emg1_filtered>threshold1) {
        Duim = 1;
    } else {
        Duim= 0;
    }
    //EMG2 threshold check
    if(emg2_filtered>threshold2) {
        Bicep = 1;
    } else {
        Bicep= 0;
    }
    //EMG3 threshold check
    if(emg3_filtered>threshold3) {
        Tricep = 1;
    } else {
        Tricep= 0;
    }
    //EMG4 threshold check
    if(emg4_filtered>threshold4) {
        Onderarm = 1;
    } else {
        Onderarm= 0;
    }



}


Ticker      sample_timer;


void sample()
{
    pc.printf("Duim Right = %i", Duim);
    pc.printf("Bicep Right = %i",Bicep);
    pc.printf("tricep Left = %i", Tricep);
    pc.printf("onderarm Left = %i", Onderarm);
    
    led = !led;
}

int main()
{   
    sample_ticker.attach(&emgsample, ts);

    //BiQuad Chain add
    highp1.add( &highp1_1 ).add( &highp1_2 );
    notch1.add( &notch1_1 ).add( &notch1_2 );
    lowp1.add( &lowp1_1 ).add(&lowp1_2);

    highp2.add( &highp2_1 ).add( &highp2_2 );
    notch2.add( &notch2_1 ).add( &notch2_2 );
    lowp2.add( &lowp2_1 ).add(&lowp2_2);

    highp3.add( &highp3_1 ).add( &highp3_2 );
    notch3.add( &notch3_1 ).add( &notch3_2 );
    lowp3.add( &lowp3_1 ).add(&lowp3_2);

    highp4.add( &highp4_1 ).add( &highp4_2 );
    notch4.add( &notch4_1 ).add( &notch4_2 );
    lowp4.add( &lowp4_1 ).add(&lowp4_2);
    
    
                temp_highest_emg1 = 0; //highest detected value right biceps
                temp_highest_emg2 = 0;
                temp_highest_emg3 = 0;
                temp_highest_emg4 = 0;

                timer_calibration.reset();
                timer_calibration.start();

                
                CalibrationEMG();
                sample_timer.attach(&sample, 0.5);
                threshold_check_ticker.attach(&threshold_check, 0.001)
                //sample_ticker.detach();
                //timer_calibration.stop();
               
    
    

    /*empty loop, sample() is executed periodically*/
    while(1) {
        wait(0.01);
        }
}