File content as of revision 3:aacea1875b7a:

#include "mbed.h"
#include "Servo.h"
#include "BiQuad.h"
 
Servo myservo(D6);

AnalogIn    emgS(A0);                                                           // EMG Servo spier
DigitalIn   CalButton(PTA4);                                                    // Button used for gaining zero and max
DigitalIn   zeromax(PTC6);                                                      // Button used for switching between zero and max

DigitalOut  ledB(LED_BLUE);                                                     // Informative LED for gaining zero and max
DigitalOut  ledR(LED_RED);
DigitalOut  ledG(LED_GREEN);                                                    // Informative LED for gaining zero and max

Ticker      emgSampleTicker;                                                    // Ticker for sample frequency 

 int R = 200;                                                                   // Number of points for movag third emg
 double C[200];                                                                 // Vector for storing data of third emg
 int k = 0;                                                                     // Counter for configuration:
 double Svector[200];                                                           // Vector for Swaarde configuration
 double Swaarde[2];                                                             // Vector for storage of max and zero of servo emg
 int x = 0;                                                                     // Variable for switching between zero and max
 double movagS;                                                                 // Moving Average mean value of servo spier                                                       
 float thresholdS = 10;                                                         

//Highpassfilter                        Fc = 10 Hz;, Q = 0.5, Fs = 500 Hz
const double b0HP = 0.8851221317817073;
const double b1HP = -1.7702442635634146;
const double b2HP = 0.8851221317817073;
const double a1HP =  -1.7632371847263784;
const double a2HP = 0.777251342400451;
//Notchfilter                           Fc = 50 Hz, Q = 10, Fs = 500 Hz
const double b0NO = 0.9714498065192796;
const double b1NO = -1.5718388053127037;
const double b2NO = 0.9714498065192796;
const double a1NO = -1.5718388053127037;
const double a2NO = 0.9428996130385592;

//BiQuad LPS( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
BiQuad HPS( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
BiQuad NOS( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 


void emgSample() {
    
    double emgNOFilteredS = NOS.step(emgS.read());                              // Filtered NO value of EMG signal servo spier                
    double emgHPFilteredS = HPS.step(emgNOFilteredS);                           // Filtered HP value of EMG signal servo spier
    double emgabsS = fabs(emgHPFilteredS);                                      // Absolute value of EMG signal servo spier
    
    for(int i = R-1; i >= 0; i--){                                              // For-loop used for moving average
        if (i == 0) {
            C[i] = emgabsS;
            }
         else {
             C[i] = C[i-1];
             }   
    }
    double sumS = 0;
    for (int n = 0; n < R-1; n++) {                                             // Summation of array
        sumS = sumS + C[n];
    }
    movagS = sumS/R;  
    
     if (CalButton==0 & k<200) {                                                 // Loop used for gaining max and zero value
        Svector[k] = movagS;
        
        if (x==0){
        ledB = !ledB;                                                           // SPIER NIET AANSPANNEN BIJ BLAUW
        }                                                                       // SPIER WEL AANSPANNEN BIJ ROOD
        else if (x==1){
        ledR = !ledR;
        }
        k++;
    }    
    else if (k==200) {                                                          // End of the loop, used for calculation value
        double sumX = 0;
            for (int n = 0; n < 199; n++) {                                     
            sumX = sumX + Svector[n];
            } 
        Swaarde[x] = sumX/200;                                                  // Value of zero for Servo spier
        k++;
        ledB = 1;
        ledR = 1;
        ledG = !ledG;
    }
    else if (k == 201 && zeromax ==0) {                                           // Loop used for switching between zero and max
        ledG = !ledG;
        k = 0;
        if (x==0) {
            x++;
        }
        else if (x==1) {
            x=0;
        }
    }
    if (x==1)                                                                   // Determining threshold using zero and max
    {
        thresholdS = Swaarde[0]+(Swaarde[1]-Swaarde[0])*(0.25f);
    }
}    




int main() {
    
ledB = 1;
ledG = 1;
ledR = 1;
emgSampleTicker.attach( &emgSample, 0.002 );                                    // Ticker for EMG function
    
    while(1) {
        
        if (movagS > thresholdS)
        {   myservo = 0.5;
            ledB = 0;
            wait(0.01);
        }
        else {
            myservo = 0.0;
            wait(0.01);
            ledB = 1;
            }
    }
}