File content as of revision 3:9cd46de17b01:

#include "mbed.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
#include <iostream>
#include "MovingAverage.h"
#define NSAMPLE 100

DigitalOut led1(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
MODSERIAL pc(USBTX, USBRX);

// Tickers
Ticker sample_ticker; //ticker voor filteren met 1000Hz
Ticker threshold_check_ticker; //ticker voor het checken van de threshold met 1000Hz
Timer timer_calibration; //timer voor EMG Kalibratie
double ts = 0.001; //tijdsstap !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
double calibration_time = 37; //Kalibratie tijd

//Input
AnalogIn    emg1( A1 ); //Duim
AnalogIn    emg2( A2 ); //Bicep
AnalogIn    emg3( A3 ); //Dorsaal
AnalogIn    emg4( A4 ); //Palmair

// GLOBALS EMG
//Gefilterde EMG signalen
volatile double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;

bool thresholdreach1 = false;
bool thresholdreach2 = false;
bool thresholdreach3 = false;
bool thresholdreach4 = false;

volatile double temp_highest_emg1 = 0; //Hoogste waarde gevonden tijdens kalibratie
volatile double temp_highest_emg2 = 0;
volatile double temp_highest_emg3 = 0;
volatile double temp_highest_emg4 = 0;

//Percentage van de hoogste waarde waar de bovenste treshold gezet moet worden
double Duim_p_t = 0.5; 
double Bicep_p_t = 0.4; 
double Dorsaal_p_t = 0.6; 
double Palmair_p_t = 0.5;

//Percentage van de hoogste waarde waar de onderste treshold gezet moet worden
double Duim_p_tL = 0.5; 
double Bicep_p_tL = 0.4; 
double Dorsaal_p_tL = 0.5; 
double Palmair_p_tL = 0.5;

// Waarde bovenste treshold waar het signaal overheen moet om de arm te activeren
volatile double threshold1;
volatile double threshold2;
volatile double threshold3;
volatile double threshold4;

// Waarde onderste treshold waar het signaal onder moet om de arm te deactiveren
volatile double threshold1L;
volatile double threshold2L;
volatile double threshold3L;
volatile double threshold4L;

// thresholdreads bools
int Duim;
int Bicep;
int Dorsaal;
int Palmair;

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


//EMG1!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp1;
BiQuad highp1_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp1_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch1_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp1( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp2;
BiQuad highp2_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp2_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch2_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp2( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG3!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp3;
BiQuad highp3_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp3_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch3_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp3( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

//EMG4!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//Highpass vierde orde cutoff 20Hz, Band filter om 49, 50, 51Hz eruit te filteren
BiQuadChain highp4;
BiQuad highp4_1( 0.8485, -1.6969, 0.8485, 1.0000, -1.7783, 0.7924 );
BiQuad highp4_2( 1.0000, -2.0000, 1.0000, 1.0000, -1.8934, 0.9085 );
BiQuad notch4_1( 0.9907, -1.8843, 0.9907, 1.0000, -1.8843, 0.9813 );

//Lowpass first order cutoff 0.4Hz
BiQuad lowp4( 0.0013, 0.0013, 0, 1.0000, -0.9975, 0 );

// ~~~~~~~~~~~~~~~~~~~EMG FUNCTIONS~~~~~~~~~~~~~~~~~~
void emgsample()
{
    
    // EMG signaal lezen
    double emgread1 = emg1.read();
    double emgread2 = emg2.read();
    double emgread3 = emg3.read();
    double emgread4 = emg4.read();
    
    // Vierde orde highpass filter + notch filter
    double emg1_highpassed = highp1.step(emgread1);
    double emg2_highpassed = highp2.step(emgread2);
    double emg3_highpassed = highp3.step(emgread3);
    double emg4_highpassed = highp4.step(emgread4);

    //Rectificatie
    double emg1_abs = abs(emg1_highpassed);
    double emg2_abs = abs(emg2_highpassed);
    double emg3_abs = abs(emg3_highpassed);
    double emg4_abs = abs(emg4_highpassed);

    //All EMG abs into lowpass
    emg1_filtered = lowp1.step(emg1_abs);
    emg2_filtered = lowp2.step(emg2_abs);
    emg3_filtered = lowp3.step(emg3_abs);
    emg4_filtered = lowp4.step(emg4_abs);

}

void CalibrationEMG()
{
    timer_calibration.reset();
    timer_calibration.start();
    while(timer_calibration<20) {                           //Duim
        if(timer_calibration>0 && timer_calibration<20) {
            led1=!led1;
            if(emg1_filtered>temp_highest_emg1) {
                temp_highest_emg1= emg1_filtered;
                pc.printf("Highest value Duim= %f \r\n", temp_highest_emg1);
            }
        
            if(emg2_filtered>temp_highest_emg2) {
                temp_highest_emg2= emg2_filtered;
                pc.printf("Highest value Bicep= %f \r\n", temp_highest_emg2);
            }
        
            if(emg3_filtered>temp_highest_emg3) {
                temp_highest_emg3= emg3_filtered;
                pc.printf("Highest value Dorsaal= %f \r\n", temp_highest_emg3);
            }
        
            if(emg4_filtered>temp_highest_emg4) {
                temp_highest_emg4= emg4_filtered;
                pc.printf("Highest value Palmair= %f \r\n", temp_highest_emg4);
            }
            
        }
        led1=1;
        led2=1;
        led3=1;

    }
    pc.printf("threshold calculation\r\n");
    threshold1 = temp_highest_emg1*Duim_p_t;  
    threshold2 = temp_highest_emg2*Bicep_p_t; 
    threshold3 = temp_highest_emg3*Dorsaal_p_t;  
    threshold4 = temp_highest_emg4*Palmair_p_t;
    
    threshold1L = temp_highest_emg1*Duim_p_tL;
    threshold2L = temp_highest_emg2*Bicep_p_tL; 
    threshold3L = temp_highest_emg3*Dorsaal_p_tL; 
    threshold4L = temp_highest_emg4*Palmair_p_tL; 
}

//Check of emg_filtered boven de threshold is
void threshold_check()
{

// EMG1 Check    
if (thresholdreach1 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
    //bovenste threshold check
    if(emg1_filtered>threshold1) {
        Duim = 1;
        thresholdreach1 = true;
        
    } else {
        Duim= 0;
    }
    }
else{                                   //Als emg_filtered boven de bovenste threshold is geweest
    //onderste threshold check
    if(emg1_filtered<threshold1L) {
        Duim = 0;
        thresholdreach1 = false;
        
    } else {
        Duim= 1;
    }
    }

// EMG2 Check    
if (thresholdreach2 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
    //bovenste threshold check
    if(emg2_filtered>threshold2) {
        Bicep = 1;
        thresholdreach2 = true;
    } else {
        Bicep= 0;
    }
    }
else{                                   //Als emg_filtered boven de bovenste threshold is geweest
    //onderste threshold check
    if(emg2_filtered<threshold2L) {
        Bicep = 0;
        thresholdreach2 = false;
        
    } else {
        Bicep= 1;
    }
    }

// EMG3 Check    
if (thresholdreach3 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
    //bovenste threshold check
    if(emg3_filtered>threshold3) {
        Dorsaal = 1;
        thresholdreach3 = true;
    } else {
        Dorsaal= 0;
    }
    }
else{                                   //Als emg_filtered boven de bovenste threshold is geweest
    //onderste threshold check
    if(emg3_filtered<threshold3L) {
        Dorsaal = 0;
        thresholdreach3 = false;
        
    } else {
        Dorsaal= 1;
    }
    }

// EMG4 Check    
if (thresholdreach4 == false){            //Als emg_filtered nog niet boven de bovenste threshold is geweest
    //bovenste threshold check
    if(emg4_filtered>threshold4) {
        Palmair = 1;
        thresholdreach4 = true;
    } else {
        Palmair= 0;
    }
    }
else{                                   //Als emg_filtered boven de bovenste threshold is geweest
    //onderste threshold check
    if(emg4_filtered<threshold4L) {
        Palmair = 0;
        thresholdreach4 = false;
        
    } else {
        Palmair= 1;
    }
    }

    }



   




Ticker      sample_timer;



void sample()
{
pc.printf("Duim Right = %i\r\n", Duim);
pc.printf("Bicep Right = %i\r\n",Bicep);
pc.printf("Dorsaal Left = %i\r\n", Dorsaal);
pc.printf("Palmair Left = %i\r\n", Palmair);           
        
}


int main()
{   
                led1 = 1;
                led2 = 1;
                led3 = 1;
    sample_ticker.attach(&emgsample, 0.001);            // Leest het ruwe EMG signaal af met een frequentie van 1000Hz
    pc.baud(115200);
    
//BiQuad Chain add
    highp1.add( &highp1_1 ).add( &highp1_2 ).add( &notch1_1 );
    highp2.add( &highp2_1 ).add( &highp2_2 ).add( &notch2_1 );
    highp3.add( &highp3_1 ).add( &highp3_2 ).add( &notch3_1 );
    highp4.add( &highp4_1 ).add( &highp4_2 ).add( &notch4_1 );

    temp_highest_emg1 = 0; //highest detected value right Biceps
    temp_highest_emg2 = 0;
    temp_highest_emg3 = 0;
    temp_highest_emg4 = 0;


    CalibrationEMG();
    pc.printf("threshold1 = %i, threshold1L = %f\r\n", threshold1, threshold1L);
    threshold_check_ticker.attach(&threshold_check, 0.01);
    led1 = 1;
    led2 = 1;
    led3 = 1;
    sample_timer.attach(&sample, 0.1);
    pc.printf("sample timer attached\r\n");

    timer_calibration.stop();



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