File content as of revision 7:763117f8ef51:

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

//Define objects
AnalogIn    emg_biceps(PTB0);
AnalogIn    emg_triceps(PTB1);
AnalogIn    emg_flexoren(PTB2);
AnalogIn    emg_extensoren(PTB3); //Analog input
PwmOut      red(LED_RED); //PWM output
Ticker timer;
MODSERIAL pc(USBTX,USBRX,64,1024);


//high pass filter constantes 15Hz cutoff 4e orde, Fs = 312,5Hz (geeft een wat mooiere waarde voor periode en is geen veelvoud van 50Hz)
#define NUM0 0.6731 // constante
#define NUM1 -2.6925 // z^-1
#define NUM2 4.0388 // z^-2etc.
#define NUM3 -2.6925
#define NUM4 0.6731

#define DEN0 1 // constante
#define DEN1 -3.2133
#define DEN2 3.9348
#define DEN3 -2.1689
#define DEN4 0.4531

//lowpass filter constantes 4Hz 4e orde, Fs = 312,5 Hz
#define NUM0_3 0.00000236 // constante
#define NUM1_3 0.00000944 // z^-1
#define NUM2_3 0.00001415 // z^-2etc.
#define NUM3_3 0.00000944
#define NUM4_3 0.00000236

#define DEN0_3 1 // constante
#define DEN1_3 -3.7899
#define DEN2_3 5.3914
#define DEN3_3 -3.4119
#define DEN4_3 0.8104

double std_dev(float value, int number) {
      int n = 20;
      static int startcount = 0;
      float sum;
      float sq_sum;
      float mean;
      float variance;
    static int count_biceps=0;
    static int count_triceps=0;
    static int count_flexoren=0;
    static int count_extensoren=0;
    static float keeper_biceps[20];
    static float keeper_triceps[20];
    static float keeper_flexoren[20];
    static float keeper_extensoren[20];
   
   if (startcount >= n)
    {switch (number){
        case 1:
            //int n = sizeof(keeper_biceps)/sizeof(float);
            keeper_biceps[count_biceps]=value;
            count_biceps++;
            if ( count_biceps >= n)
                count_biceps = 0;
            double sum = 0;
            double sq_sum = 0;
            for(int i = 0; i < n; ++i) {
                sum += keeper_biceps[i];
                sq_sum += keeper_biceps[i] * keeper_biceps[i];
            }
            double mean = sum / n;
            double variance = sq_sum / n - mean * mean;
            return sqrt(variance);
            break;
            
        case 2:
            //int n = sizeof(keeper_triceps)/sizeof(float);
            keeper_triceps[count_triceps]=value;
            count_triceps++;
            if ( count_triceps >= n)
                count_triceps = 0;
            sum = 0;
            sq_sum = 0;
            for(int i = 0; i < n; ++i) {
                sum += keeper_triceps[i];
                sq_sum += keeper_triceps[i] * keeper_triceps[i];
            }
            mean = sum / n;
            variance = sq_sum / n - mean * mean;
            return sqrt(variance);
            break;

        case 3:
            //n = sizeof(keeper_flexoren)/sizeof(float);
            keeper_flexoren[count_flexoren]=value;
            count_flexoren++;
            if ( count_flexoren >= n)
                count_flexoren = 0;
            sum = 0;
            sq_sum = 0;
            for(int i = 0; i < n; ++i) {
                sum += keeper_flexoren[i];
                sq_sum += keeper_flexoren[i] * keeper_flexoren[i];
            }
            mean = sum / n;
            variance = sq_sum / n - mean * mean;
            return sqrt(variance);
            break;

        case 4:
            //n = sizeof(keeper_extensoren)/sizeof(float);
            keeper_extensoren[count_extensoren]=value;
            count_extensoren++;
            if ( count_extensoren >= n)
                count_extensoren = 0;
            sum = 0;
            sq_sum = 0;
            for(int i = 0; i < n; ++i) {
                sum += keeper_extensoren[i];
                sq_sum += keeper_extensoren[i] * keeper_extensoren[i];
            }
            mean = sum / n;
            variance = sq_sum / n - mean * mean;
            return sqrt(variance);
            break;
    } // einde switch
   } // einde if
   else
            {startcount+=1;
            switch(number){
            case 1:
                keeper_biceps[count_biceps]=value;
                count_biceps++;
                if(count_biceps >= n)
                    count_biceps=0;
                break;
            case 2:
                keeper_triceps[count_triceps]=value;
                count_triceps++;
                if(count_triceps >= n)
                    count_triceps=0;
                break;
            case 3:
                keeper_flexoren[count_flexoren]=value;
                count_flexoren++;
                if(count_flexoren >= n)
                    count_flexoren=0;
                break;
            case 4:
                keeper_extensoren[count_extensoren]=value;
                count_extensoren++;
                if(count_extensoren >= n)
                    count_extensoren=0;
                break;    
            } // einde switch
        } // einde else
}

float filter(int sig_number){
    float sig_out;
    // eerst variabelen definieren
    //biceps
        //filter 1
    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;

        //filter 3
    float in0_3_biceps =0;
    static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
    static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;

    //triceps
        //filter 1
    float in0_triceps =0;
    static float 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;

        //filter 3
    float in0_3_triceps =0;
    static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
    static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;

    //flexoren
        //filter 1
    float in0_flexoren =0;
    static float 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;
       
        //filter 3
    float in0_3_flexoren =0;
    static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
    static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;

    //extensoren
        //filter 1
    float in0_extensoren =0;
    static float 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;

        //filter 3
    float in0_3_extensoren =0;
    static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
    static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;

    switch(sig_number){            
        case 1:        
            // signaal filteren op 15 Hz HIGHPASS
            in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
            in0_biceps = emg_biceps.read() - offset_biceps;
            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;                      
            
            //signaal filteren op 5Hz LOWPASS
            /*in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
            in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3)
            out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;           
            out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
*/
            sig_out = out0_biceps;
            break;

        case 2:
            // signaal filteren op 15 Hz HIGHPASS
            in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
            in0_triceps = emg_triceps.read() - offset_biceps;
            out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;           
            out0_triceps = (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;                      

            //signaal filteren op 5Hz LOWPASS
            /*in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
            in0_3_triceps = abs(out0_triceps);
            out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;           
            out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;    
*/
            sig_out = out0_triceps;
            break;

        case 3:
            // signaal filteren op 15 Hz HIGHPASS
            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;                      

            //signaal filteren op 5Hz LOWPASS
            /*in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
            in0_3_flexoren = abs(out0_flexoren);
            out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;           
            out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;    
*/
            sig_out = out0_flexoren;
            break;

        case 4:
            // signaal filteren op 15 Hz HIGHPASS
            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;                      

            //signaal filteren op 5Hz LOWPASS
            /*in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
            in0_3_extensoren = abs(out0_extensoren);
            out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;           
            out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;    
*/
            sig_out = out0_extensoren;
            break;
    }
    return sig_out;
}

void looper()
{   float emg_value_biceps;
    float emg_value_triceps;
    float emg_value_flexoren;
    float emg_value_extensoren;
    float dy;
    float dx;
    //static int sig_count = 1;

    emg_value_biceps = std_dev(filter(1),1);
    emg_value_triceps = std_dev(filter(2),2); 
    emg_value_flexoren = std_dev(filter(3),3);
    emg_value_extensoren = std_dev(filter(4),4);

 
    if(emg_value_biceps < 4.5)
        emg_value_biceps=0;
    else if (emg_value_biceps > 13)
         emg_value_biceps = 13;
    emg_value_biceps = 1.5 * emg_value_biceps;

    if(emg_value_triceps < 4.5)
        emg_value_triceps=0;
    else if (emg_value_triceps > 10)
         emg_value_triceps = 10;
    emg_value_triceps = emg_value_triceps;
    
    if(emg_value_flexoren < 3) // was 2, wordt 3
        emg_value_flexoren=0;
    else if (emg_value_flexoren > 8)
         emg_value_flexoren = 8;
    emg_value_flexoren = 1.5*emg_value_flexoren; // was 2, wordt 1.5
        
    if(emg_value_extensoren < 5)
        emg_value_extensoren=0;
    else if (emg_value_extensoren > 13)
         emg_value_extensoren = 13;

    dy = (emg_value_biceps - emg_value_triceps) / 30 - .5; // output biceps en triceps tussen 0 en 15
    dx = (emg_value_flexoren - emg_value_extensoren) / 30 -.5; // nu dx en dy waarden tussen 1 en 0, net als de potmeters
 
    if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
        pc.printf("%.6f, %.6f\n",dy,dx);
        
    
        
    /**When not using the LED, the above could also have been done this way:
    * pc.printf("%.6\n", emg0.read());
    */
}

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);
    /**Here you attach the 'void looper(void)' function to the Ticker object
    * The looper() function will be called every 1/Fs seconds.
    * Please mind that the parentheses after looper are omitted when using attach.
    */
    timer.attach(looper,0.0032); //invullen in seconde. .0032 is niet eens afgerond, dus vandaar die frequentie.
    while(1) //Loop
    {
      /*Empty!*/
      /*Everything is handled by the interrupt routine now!*/
    }
}