File content as of revision 1:fb33955ca402:

#include "mbed.h"
#include "MODSERIAL.h"

//Define objects
AnalogIn    emg_biceps(PTB0); //Analog input
AnalogIn    emg_triceps(PTB1);
// AnalogIn    emg_flexoren(PTB2);
// AnalogIn    emg_extensoren(PTB3);
PwmOut      red(LED_RED); // sig_out biceps
PwmOut      blue(LED_BLUE); // sig_out triceps
// PwmOut      green(LED_GREEN); 

Ticker timer;
MODSERIAL pc(USBTX,USBRX,64,1024);

#define MAXCOUNT 40
#define inertia 4

#define gain_biceps 1
#define threshold_biceps 0.04
#define border_biceps 0.1125 //25% van .57 - .12 

#define gain_triceps 1
#define threshold_triceps 0.1
#define border_triceps 0.1237 //25% van .605 - .11 = .12375

#define NUM0 0.8841 // constante
#define NUM1 -3.53647 // z^-1
#define NUM2 5.3046 // z^-2etc.
#define NUM3 -3.5364
#define NUM4 0.8841

#define DEN0 1 // constante
#define DEN1 -3.7538
#define DEN2 5.2912
#define DEN3 -3.3189
#define DEN4 0.7816

//filter functie definieren. filter(signal_number)
//signal_number=1 --> biceps filteren
//signal_number=2 --> triceps filteren
//signal_number=3 --> flexoren filteren
//signal_number=4 --> extensoren filteren

float filter(int signal_number){ 
    //static variables keep their values between function calls
    //the assignents are only executed the first iteration.
           
    //variabelen biceps definieren
    float in0_biceps =0;
    static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
    static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
   
    // variabelen triceps definieren
    static float in0_triceps = 0, in1_triceps = 0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
    static float out0_triceps = 0, out1_triceps = 0, out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
  
    //variabelen flexoren definieren
    static float in0_flexoren = 0, in1_flexoren = 0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
    static float out0_flexoren = 0, out1_flexoren = 0, out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
   
    //variablen extensoren definieren
    static float in0_extensoren = 0, in1_extensoren = 0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
    static float out0_extensoren = 0, out1_extensoren = 0, out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0; 
    
    //overige variabelen definieren
    // verwijder static waar mogelijk, maakt programma iets sneller
    static float count_biceps = 0, count_triceps = 0, count_extensoren = 0, count_flexoren = 0;
    static float square_biceps = 0, square_triceps = 0, square_flexoren = 0, square_extensoren = 0;
    static float sum_biceps = 0, sum_triceps = 0, sum_flexoren = 0, sum_extensoren = 0; 
    static float mean_biceps = 0.1, mean_triceps = 0.1, mean_flexoren = 0.1, mean_extensoren = 0.1;
    static float emg_biceps, emg_triceps, emg_flexoren, emg_extensoren; // output ruwe EMG
    static float sig_in_biceps, sig_in_triceps, sig_in_extensoren, sig_in_flexoren; // naam gewijzigd, output StDev
    static float sig_out_biceps, sig_out_triceps, sig_out_extensoren, sig_out_flexoren;
    float emg_abs, sig_out, deltaV;
    static float sig_prev_biceps = 0, sig_prev_triceps = 0, sig_prev_extensoren = 0, sig_prev_flexoren = 0;
    static int stat0_biceps, stat0_triceps, stat0_extensoren, stat0_flexoren;
    static int stat1_biceps = 0, stat1_triceps = 0, stat1_extensoren = 0, stat1_flexoren = 0;
 
    switch (signal_number){
        case 1:
            //biceps filteren
            in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
            in0_biceps = emg_biceps.read();
            out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;           
            out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;                      
            
            //std deviatie bepalen, om de N metingen
            emg_abs = fabs(out0_biceps);
            sum_biceps += out0_biceps;
            square_biceps += (emg_abs - mean_biceps)*(emg_abs - mean_biceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
            // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
            count_biceps += 1; // hou bij hoeveel squares er zijn opgeteld
            if (count_biceps >= MAXCOUNT)
                {   sig_in_biceps = sqrt(square_biceps/count_biceps);
                    mean_biceps = sum_biceps/count_biceps;
                    count_biceps = 0; square_biceps = 0; sum_biceps = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
                    // nieuw:
                    deltaV = sig_in_biceps - sig_prev_biceps;
                    if (sig_in_biceps <= threshold_biceps) // threshold
                    {   stat0_biceps = 0;
                        if (stat1_biceps == 1)
                        sig_out_biceps = sig_prev_biceps + deltaV / inertia;
                        else sig_out_biceps = 0;
                    }
                    else if ( deltaV >= border_biceps ) // stijging
                    {   stat0_biceps = 1;
                        if (stat1_biceps == -1)
                        sig_out_biceps = sig_prev_biceps + deltaV / inertia;
                        else sig_out_biceps = sig_in_biceps;
                    }
                    else if ( deltaV <= -border_biceps ) // daling
                    { stat0_biceps = -1;
                        if (stat1_biceps == 1)
                        sig_out_biceps = sig_prev_biceps + deltaV / inertia;
                        else sig_out_biceps = sig_in_biceps;
                    }
                    else { stat0_biceps = 0;
                         sig_out_biceps = sig_in_biceps;
                         }
            sig_prev_biceps = sig_in_biceps;
            stat1_biceps = stat0_biceps;
            sig_out = sig_out_biceps;
            red = sig_out_biceps;
            }
            else sig_out = -1;     
            break;
        case 2:       
            //triceps filteren
            in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
            in0_triceps = emg_triceps.read();
            out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
            out0_biceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0; 
            
            //std deviatie bepalen om de N metingen
            emg_abs = fabs(out0_triceps);
            sum_triceps += out0_triceps;
            square_triceps += (emg_abs - mean_triceps)*(emg_abs - mean_triceps); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
            // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
            count_triceps += 1; // hou bij hoeveel squares er zijn opgeteld
            
            if (count_triceps >= MAXCOUNT)
                {   sig_in_triceps = sqrt(square_triceps/count_triceps);
                    mean_triceps = sum_triceps/count_triceps;
                    count_triceps = 0; square_triceps = 0; sum_triceps = 0;// en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
                    deltaV = sig_in_triceps - sig_prev_triceps;
                    if (sig_in_triceps <= threshold_triceps) // threshold
                    {   stat0_triceps = 0;
                        if (stat1_triceps == 1)
                        sig_out_triceps = sig_prev_triceps + deltaV / inertia;
                        else sig_out_triceps = 0;
                    }
                    else if ( deltaV >= border_triceps ) // stijging
                    {   stat0_triceps = 1;
                        if (stat1_triceps == -1)
                        sig_out_triceps = sig_prev_triceps + deltaV / inertia;
                        else sig_out_triceps = sig_in_triceps;
                    }
                    else if ( deltaV <= -border_triceps ) // daling
                    { stat0_triceps = -1;
                        if (stat1_triceps == 1)
                        sig_out_triceps = sig_prev_triceps + deltaV / inertia;
                        else sig_out_triceps = sig_in_triceps;
                    }
                    else { stat0_triceps = 0;
                         sig_out_triceps = sig_in_triceps;
                         }
            sig_prev_triceps = sig_in_triceps;
            stat1_triceps = stat0_triceps;
            sig_out = sig_out_triceps;
            blue = sig_out_triceps;
                 }
            else sig_out = -1;
            break;
        case 3:
            //flexoren filteren
            in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
            in0_flexoren = emg_flexoren.read();
            out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
            out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren) / DEN0; 
                        
            //std deviatie bepalen om de N metingen
            emg_abs = fabs(out0_flexoren);
            sum_flexoren += out0_flexoren;
            square_flexoren += (emg_abs - mean_flexoren)*(emg_abs - mean_flexoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
            // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
            count_flexoren += 1; // hou bij hoeveel squares er zijn opgeteld
            if (count_flexoren >= MAXCOUNT)
                {   sig_in_flexoren = sqrt(square_flexoren/count_flexoren);
                    mean_flexoren = sum_flexoren/count_flexoren;
                    count_flexoren = 0; square_flexoren = 0; sum_flexoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
                    
                    sig_out = sig_out_flexoren;
                 }
            else sig_out = -1;
            break;
        case 4:
            //extensoren filteren
            in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren; 
            in0_extensoren = emg_extensoren.read();
            out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren; 
            out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren) / DEN0;
                       
            //std deviatie bepalen om de 50 metingen
            emg_abs = fabs(out0_extensoren);
            sum_extensoren += out0_extensoren;
            square_extensoren += (emg_abs - mean_extensoren)*(emg_abs - mean_extensoren); //neem absolute waarde, kwadrateer, voeg toe aan vorige square
            // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
            count_extensoren += 1; // hou bij hoeveel squares er zijn opgeteld
            if (count_extensoren >= MAXCOUNT)
                {   sig_in_extensoren = sqrt(square_extensoren/count_extensoren);
                    mean_extensoren = sum_extensoren/count_extensoren;
                    count_extensoren = 0; square_extensoren = 0; sum_extensoren = 0; // en neem de RMS als er genoeg zijn geteld, stuur die door, en reset sqaure en count
                    
                    sig_out=sig_out_extensoren;
                }
            else sig_out = -1;
            break;
       }
        
    return sig_out;  
   
}

void looper()
{
static float biceps, triceps, extensoren, flexoren, emg_filter_test;

    emg_filter_test = filter(1);
    if (emg_filter_test != -1) biceps = emg_filter_test;
    emg_filter_test = filter(2);
    if (emg_filter_test != -1) triceps = emg_filter_test;
    /*emg_filter_test = filter(3);
    if (emg_filter_test != -1) flexoren = emg_filter_test;
    emg_filter_test = filter(4);
    if (emg_filter_test != -1) extensoren = emg_filter_test;
    */
}

int main()
{
    /*setup baudrate. Choose the same in your program on PC side*/
    pc.baud(115200);
    /*set the period for the PWM to the red LED*/
    red.period_ms(80); // periode pwm = 2*Fs , blijkbaar.
    blue.period_ms(80);


    /**Here you attach the 'void looper(void)' function to the Ticker object0
    * The looper() function will be called every 0.001 seconds.
    * Please mind that the parentheses after looper are omitted when using attach.
    */ 
    timer.attach(looper, 0.001);
    while(1) // Loop
    {   
            // zie 'encoder' , timer bepaalt wanneer een if in de while-loop berekeningen doet voor de motor.
        
    }
}