File content as of revision 38:be38b9318849:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "biquadFilter.h" //Filter direct form II

//      [DEFINE INPUTS]       //
DigitalOut  debug_led_red(LED_RED);                 // Debug LED
DigitalOut  debug_led_green(LED_GREEN);             // Debug LED 
DigitalOut  debug_led_blue(LED_BLUE);               // Debug LED

const double  off=1;                            // Led off
const double  on=0;                             // Led on

//      [DEFINE CONSTANTS]      //
HIDScope scope(3); // Aantal HIDScope kanalen
MODSERIAL pc(USBTX,USBRX);
Ticker control_tick;
Ticker T1;

//      [BIQUAD FILTERS]        //
int Fs = 512; // sampling frequency
const double low_b0 = 0.05892937945281792;
const double low_b1 = 0.11785875890563584;
const double low_b2 = 0.05892937945281792;
const double low_a1 = -1.205716572226748;
const double low_a2 = 0.44143409003801976; //Notch 1 LOW: VIA online biquad calculator Lowpass 520-48-0.7071-6

const double high_b0 = 0.6389437261127494;
const double high_b1 = -1.2778874522254988;
const double high_b2 = 0.6389437261127494;
const double high_a1 = -1.1429772843080923;
const double high_a2 = 0.41279762014290533; // NOTCH 2 HIGH: VIA online biquad calculator Highpas 520-52-0.7071-6

const double high_b0_HP = 0.6389437261127494;
const double high_b1_HP = -1.2778874522254988;
const double high_b2_HP = 0.6389437261127494;
const double high_a1_HP = -1.1429772843080923;
const double high_a2_HP = 0.41279762014290533; // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz

const double low_b0_LP = 0.05892937945281792;
const double low_b1_LP = 0.11785875890563584;
const double low_b2_LP = 0.05892937945281792;
const double low_a1_LP = -1.205716572226748;
const double low_a2_LP = 0.44143409003801976; // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz
//
//Left bicep
biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden? (>25Hz doorlaten)
biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); 

// Right bicep
biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden?
biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2);  // moeten nog waardes voor gemaakt worden
biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);// moeten nog waardes voor gemaakt worden?


AnalogIn input1(A0);                                          // EMG: Two AnalogIn EMG inputs
AnalogIn input2(A1);                                          // EMG: Two AnalogIn EMG inputs

//                     __________________________
//                    :     [EMG variables]      : 
//                    :__________________________:
//

volatile bool control_go = false;                             // EMG: 
volatile bool sample = false;
volatile bool sample_error = false;
volatile bool sample_error_strike = false;

double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;    
double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;

double minimum_L;                      double maximum_L;                                           
double EMG_L_min;                      double EMG_L_max;                                    
double minimum_R;                      double maximum_R;                                              
double EMG_R_min;                      double EMG_R_max;                                                                                                                 
double EMG_left_Bicep;                 double EMG_Right_Bicep;                   // input variables
double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1; 
double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2; 
double EMG_Left_Bicep_filtered;        double EMG_Right_Bicep_filtered;          // output variables


double n=0; double c=0; double k=0; double p=0; double er=0;

// FUNCTIONS
void ControlGo();
void take_sample();
void Filter();
void sample_filter();
void countdown_from_5();
void calibration();
void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black();


//==========================//
//      [MAIN FUNCTION]     //
//==========================//
int main()
{
    control_tick.attach(&ControlGo, (float)1/Fs);
    pc.baud(115200);
    double n=0;
    // calibration(); // calibreer min en max positie
    while(1)
    {
        if(control_go)
        {
            n++;
            pc.printf("n %f",n);
            sample_filter();
            scope.set(0,n);                  //left bicep unfiltered
            scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
            scope.set(2,moving_average_left);     //
            scope.send();
            control_go = 0;
        } 
        yellow();
    } // while end   
} // main end

// --------------------------------------------------------------------------------------------------------
//      [FUNCTIONS]        //
void ControlGo() //Control flag
{
    control_go = true;
}

void sample_filter()
{
    Filter();
    take_sample();
    if(sample)
        {
            sample=false;
            Sample_EMG_L_1 = EMG_Left_Bicep_filtered;               Sample_EMG_R_1 = EMG_Right_Bicep_filtered; 
            
            Sample_EMG_L_10= Sample_EMG_L_9;                        Sample_EMG_R_10= Sample_EMG_R_9;
            Sample_EMG_L_9 = Sample_EMG_L_8;                        Sample_EMG_R_9 = Sample_EMG_R_8;
            Sample_EMG_L_8 = Sample_EMG_L_7;                        Sample_EMG_R_8 = Sample_EMG_R_7;
            Sample_EMG_L_7 = Sample_EMG_L_6;                        Sample_EMG_R_7 = Sample_EMG_R_6;
            Sample_EMG_L_6 = Sample_EMG_L_5;                        Sample_EMG_R_6 = Sample_EMG_R_5;
            Sample_EMG_L_5 = Sample_EMG_L_4;                        Sample_EMG_R_5 = Sample_EMG_R_4;
            Sample_EMG_L_4 = Sample_EMG_L_3;                        Sample_EMG_R_4 = Sample_EMG_R_3;
            Sample_EMG_L_3 = Sample_EMG_L_2;                        Sample_EMG_R_3 = Sample_EMG_R_2;
            Sample_EMG_L_2 = Sample_EMG_L_1;                        Sample_EMG_R_2 = Sample_EMG_R_1;
        }
        moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1;
        moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1;  
    n++;   
}

void take_sample() // Take a sample every 25th sample
{
    if(n==25)
    {
    sample = true; n=0;
    }
    
     if(er==5)
    {
    sample_error = true; er=0;
    }
    
    sample_error_strike = true;
}

                                //    [FILTER FUNCTIONS]    //
                                //          [EMG]           //
                                  
void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
    {
    EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
    
    EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
    EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
    
    EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
    EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
    
    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
    }
    
void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
    {
    wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r");
    wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
    }
    
void calibration()
{
    
    
                        //  [MINIMUM VALUE BICEPS CALIBRATION]   //

    pc.printf("Start minimum calibration in 5 seconds \n\r");
    pc.printf("Keep your biceps as relaxed as possible \n\r");
    
    countdown_from_5();
    c=0;
    
    while(c<2560)                    // 512Hz -> 2560 is equal to five seconds
    {
    Filter();                        // Filter EMG signal
    minimum_L=EMG_Left_Bicep_filtered+minimum_L;          // Take previous sample EMG_Left_Bicep_filtered and add the new value
    minimum_R=EMG_Right_Bicep_filtered+minimum_R;   
//    scope.set(0,EMG_left_Bicep);
//    scope.set(1,EMG_Left_Bicep_filtered);
//    scope.set(2,minimum_L);
//    scope.send();
    c++;                             // Every sample c is increased by one until the statement c<2560 is false
    wait(0.001953125);               // wait one sample
    }
    
    pc.printf("Finished minimum calibration \n\r");
    
    EMG_L_min=minimum_L/2560;        // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
    EMG_R_min=minimum_R/2560;
    
    pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);

    wait (3);                        //cooldown
    
    
                    //  [MAXIMUM VALUE BICEPS CALIBRATION]   //
                    
                    
    pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
    
    countdown_from_5();
    c=0;
    
    while(c<2560)              // 512Hz -> 2560 is equal to five seconds
    {
    Filter();                  // Filter EMG signal
    maximum_L=EMG_Left_Bicep_filtered+maximum_L;    // Take previous sample EMG_Left_Bicep_filtered and add the new value
    maximum_R=EMG_Right_Bicep_filtered+maximum_R;     
    c++;                       // Every sample c is increased by one until the statement c<2560 is false
    wait(0.001953125);
    }
    
    pc.printf("Finished minimum calibration \n\r");
    
    EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
    EMG_R_max=maximum_R/2560;
    
    pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f  \n\r", EMG_L_max, EMG_R_max);

    wait (3); //cooldown
    
    
                     //  [MAXIMUM VALUE BICEPS CALIBRATION]   //
                     //   Calculate threshold percentages     //
                     
    const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;;      //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
    const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min;      //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); 
    const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min;     //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
    const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min;     //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);

    pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f  \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
    
}

void red()      { debug_led_red=on;     debug_led_blue=off;  debug_led_green=off;   }
void blue()     { debug_led_red=off;    debug_led_blue=on;   debug_led_green=off;   }
void green()    { debug_led_red=off;    debug_led_blue=off;  debug_led_green=on;    }
void white()    { debug_led_red=on;     debug_led_blue=on;   debug_led_green=on;    }
void yellow()   { debug_led_red=on;     debug_led_blue=off;  debug_led_green=on;    }
void cyan()     { debug_led_red=off;    debug_led_blue=on;   debug_led_green=on;    }
void purple()   { debug_led_red=on;     debug_led_blue=on;   debug_led_green=off;   }
void black()    { debug_led_red=off;    debug_led_blue=off;  debug_led_green=off;   }