File content as of revision 4:5ceb8f058874:

#include "mbed.h"
#include "math.h"
#include "BiQuad.h"
//#include "Servo.h"
#include "HIDScope.h"



//Define objects
AnalogIn    emgL(A0);                                                           // EMG Left Arm
AnalogIn    emgR(A1);                                                           // EMG Right Arm
AnalogIn    emgS(A2);                                                           // EMG Servo spier
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
DigitalIn   CalButton(PTA4);                                                    // Button used for gaining zero and max
DigitalIn   zeromax(PTC6);                                                      // Button used for switching between zero and max
Ticker      emgSampleTicker;                                                    // Ticker for sample frequency
HIDScope    scope( 6 );                                                       
//Servo myservo(D13);
DigitalOut motor1direction(D7);
PwmOut motor1control(D6);


 int P= 200;                                                                    // Number of points for movag first emg
 int Q = 200;                                                                   // Number of points for movag second emg
 int R = 200;                                                                   // Number of points for movag third emg
 double A[200];                                                                 // Vector for storing data of first emg
 double B[200];                                                                 // Vector for storing data of second emg
 double C[200];                                                                 // Vector for storing data of third emg
 int k = 0;                                                                     // Counter for configuration:
 double Lvector[200];                                                           // Vector for Rwaarde configuration
 double Lwaarde[2];                                                             // Vector for storage of max and zero of left biceps
 double Rvector[200];                                                           // Vector for Lwaarde configuration
 double Rwaarde[2];                                                             // Vector for storage of max and zero of right biceps
 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 movagR;                                                                 // Moving Average mean value of right biceps
 double movagL;                                                                 // Moving Average mean value of left biceps
 double movagS;                                                                 // Moving Average mean value of servo spier
 float thresholdL = 10;                                                         // Startvalue for threshold, to block signals -
 float thresholdR = 10;                                                         // - before the zero and max values are calculated
 float thresholdS = 10;                                                         //-------
 
 //------------Filter parameters----------------------

//Lowpassfilter
//const double b0LP = 0.0014831498359569692;
//const double b1LP = 0.0029662996719139385;
//const double b2LP = 0.0014831498359569692;
//const double a1LP = -1.918570032544273;
//const double a2LP = 0.9245026318881009;
//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;

//--------------Filter------------
//BiQuad LPR( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
BiQuad HPR( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
BiQuad NOR( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad 

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

//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() {                                                              // EMG function
    
    double emgNOFilteredL = NOL.step(emgL.read());                              // Filtered NO value of EMG signal left biceps                
    double emgHPFilteredL = HPL.step(emgNOFilteredL);                           // Filtered HP value of EMG signal left biceps
    double emgabsL = fabs(emgHPFilteredL);                                      // Absolute value of EMG signal left biceps  
      
                                                             
    double emgNOFilteredR = NOR.step(emgR.read());                              // Filtered NO value of EMG signal right biceps                
    double emgHPFilteredR = HPR.step(emgNOFilteredR);                           // Filtered HP value of EMG signal right biceps
    double emgabsR = fabs(emgHPFilteredR);                                      // Absolute value of EMG signal right biceps          
    
    
    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
    
    
    
    
//-------------------Linker bicep-------------------------------------    
    
    for(int i = P-1; i >= 0; i--){                                              // For-loop used for moving average
        if (i == 0) {
            A[i] = emgabsL;
            }
         else {
             A[i] = A[i-1];
             }   
    }
    double sumL = 0;
    for (int n = 0; n < P-1; n++) {                                             // Summation of array
        sumL = sumL + A[n];
    }
    movagL = sumL/P;     
    
    
    
//--------------Rechter bicep---------------------------------------
    
    for(int i = Q-1; i >= 0; i--){                                              // For-loop used for moving average
        if (i == 0) {
            B[i] = emgabsR;
            }
         else {
             B[i] = B[i-1];
             }   
    }
    double sumR = 0;

    for (int n = 0; n < Q-1; n++) {                                             // Summation of array
    sumR = sumR + B[n];
    }
    movagR = sumR/Q;                                                             // Moving average value right biceps
    
//---------------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;     
    
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, emgL.read());
    scope.set(1, movagL);
    scope.set(2, emgR.read());
    scope.set(3, movagR);
    scope.set(4, emgS.read());
    scope.set(5, movagS);
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();                                                     // Moving average value left biceps


    if (CalButton==0 & k<200) {                                                 // Loop used for gaining max and zero value
        Lvector[k] = movagL;
        Rvector[k] = movagR;
        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 sumY = 0;
        double sumZ = 0;
        double sumX = 0;
            for (int n = 0; n < 199; n++) {                                     
            sumY = sumY + Lvector[n];
            sumZ = sumZ + Rvector[n];
            sumX = sumX + Svector[n];
            } 
        Lwaarde[x] = sumY/200;                                                  // Value of zero for left biceps
        Rwaarde[x] = sumZ/200;                                                  // Value of zero for right biceps
        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
    {
        thresholdL = Lwaarde[0]+(Lwaarde[1]-Lwaarde[0])*(0.25f);
        thresholdR = Rwaarde[0]+(Rwaarde[1]-Rwaarde[0])*(0.25f);
        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
Lwaarde[0] = 0.01;                                                              // Startvalue for zerovalue, to - 
Rwaarde[0] = 0.01;                                                              // - prevent dividing by 0
Swaarde[0] = 0.01;
    while(1) {
        
     //   if (movmeanR > thresholdR)
//        {   myservo = 0.5;
//            wait(0.01);
//        }
//        else {
//            myservo = 0.0;
//            wait(0.01);
//            }
//            
//        if (movmeanL > thresholdL)
//        {   motor1control.write(0.8);
//            motor1direction.write(true);
//            }
//        else {
//            motor1control.write(0);
//            }
    }
}