File content as of revision 1:04e5611501f6:

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

//Define objects
AnalogIn    emg_biceps(PTB0); //Analog input
PwmOut      red(LED_RED); // EMG meting
// PwmOut      blue(LED_BLUE); // uitgangssignaal controle
// PwmOut      green(LED_GREEN); 

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

#define gain_biceps 10 // nog niet gebruikt.
#define threshold_biceps 0.04
#define border_biceps 0.1125
#define maxcount 40 // kies niet te groot, anders werkt rem niet snel genoeg. het kost maxcount*2/1000 seconde om van richting te veranderen.
#define inertia 4

#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

/* hou in de gaten welke waarden globaal gedefinieerd moeten worden*/ 
float count = 0, square_biceps = 0, sum_biceps = 0, mean_biceps = 0;
// mean_biceps wel of niet constant nemen?

void looper()
{
    /*value0 is huidig, 1 is t-1, 2 is t-2 etc. Gebruik later aanduidingen ABCD. */   
    float mean;
    static float in0 = 0, in1 = 0, in2 = 0, in3 = 0, in4 = 0;
    static float out0 = 0, out1 = 0, out2 = 0, out3 = 0, out4 = 0;
    
    in4 = in3; in3 = in2; in2 = in1; in1 = in0;
    in0 = emg_biceps.read();
    red = in0;
    /* rode led voor meting emg*/ 
    out4 = out3; out3 = out2; out2 = out1; out1 = out0;
    out0 = (NUM0*in0 + NUM1*in1 + NUM2*in2 + NUM3*in3 + NUM4*in4 - DEN1*out1 - DEN2*out2 - DEN3*out3 - DEN4*out4 ) / DEN0;
       
    /*send value to PC. use 6 digits after decimal sign*/
    //if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30) // ! Testen filter: gebruik de if om de serial optimaal te gebruiken.
        //pc.printf("%.6f\n",emg_out_biceps);
    /**When not using the LED, the above could also have been done this way:
    * pc.printf("%.6\n", emg0.read());
    */
    
    mean = mean_biceps;
    sum_biceps += out0;
    square_biceps += (out0 - mean)*(out0 - mean); // WAS EERST (out0 - mean)^2 neem absolute waarde, kwadrateer, voeg toe aan vorige square
    // voeg rest EMG's toe, variabelen alleen _spier geven als het nodig is.
    count += 1; // hou bij hoeveel squares er zijn opgeteld
}

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(2); // periode pwm = 2*Fs , blijkbaar.
    // blue.period_ms(2);
    /**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.
    */ 
    float sig_in_biceps; float sig_out_biceps;
    static float sig_prev_biceps = 0;
    float dV_biceps;
    /* rem: eerst threshold na STD, threshold van .03 lijkt prima in testmetingen (voor biceps).
    Als onder threshold, output is nul, maar waarde wel onthouden, negeer rem verder. -> else if structuur
    als terug in threshold terwijl stat1 = 1, dan inertia, om trilling bij verlaten threshold tegen te gaan.
    status is -1, 0 of 1.
    biceps: threshold 0.12 , max is 0.57 , speling = .57-.12 = 4.5 , 25% van 4,5 = 1.125
    sigin < threshold -> stat0 = 0 , stat1 == 1 ? JA dan remmen, anders trillend rond threshold, NEE dan sigout = 0
    dV > 1.125 -> stat0 = 1 , stat1 == -1 ? JA dan remmen, NEE dan sigout=sigin
    dV < -1.125 -> stat0 = -1 , stat1 == 1 ? Ja dan remen, NEE dan sigout=sigin 
    binnen grenzen: status0 = 0, voorlopig geen verdere vragen. */
    int stat0_biceps; // huidige status
    int stat1_biceps = 0; // vorige status
    timer.attach(looper, 0.001);
    while(1) // Loop
    {   if (count >= maxcount)
        {   sig_in_biceps = sqrt(square_biceps/count);
        dV_biceps = sig_in_biceps - sig_prev_biceps;
        mean_biceps = sum_biceps/count;
        count= 0; square_biceps = 0; sum_biceps = 0; // en neem de STD als er genoeg zijn geteld, stuur die door, en reset sqaure en count
            if (sig_in_biceps <= threshold_biceps) // threshold
                { stat0_biceps = 0;
                 if (stat1_biceps == 1)
                    sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
                 else sig_out_biceps = 0;
                }
            else if ( dV_biceps >= border_biceps ) // stijging
                { stat0_biceps = 1;
                 if (stat1_biceps == -1)
                 sig_out_biceps = sig_prev_biceps + dV_biceps / inertia;
                 else sig_out_biceps = sig_in_biceps;
                }
            else if ( dV_biceps <= -border_biceps ) // daling
                { stat0_biceps = -1;
                 if (stat1_biceps == 1)
                 sig_out_biceps = sig_prev_biceps + dV_biceps / 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;
            if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
                pc.printf("%.6f\n",sig_in_biceps); // verwissel in en out om verschillende delen te testen.
         }
           
    }
}