File content as of revision 3:be5ac89a0b53:

#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
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   TestButton(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( 4 );                                                       
Servo myservo(D13);
DigitalOut motor1direction(D7);
PwmOut motor1control(D6);


 int P= 200;                                                                    // Number of points for moving average first emg
 int Q = 200;                                                                   // Number of points for moving average second emg
 double A[200];                                                                 // Vector for storing data of first emg
 double B[200];                                                                 // Vector for storing data of second emg
 int k = 0;                                                                     // Counter for configuration:
 double Rvector[200];                                                           // Vector for Rwaarde configuration
 double Rwaarde[2];                                                             // Vector for storage of max and zero of left biceps
 double Lvector[200];                                                           // Vector for Lwaarde configuration
 double Lwaarde[2];                                                             // Vector for storage of max and zero of right biceps
 int x = 0;                                                                     // Variable for switching between zero and max
 double movmeanR;                                                               // Moving Average mean value of right biceps
 double movmeanL;                                                               // Moving Average mean value of left biceps
 float thresholdL = 10;                                                         // Startvalue for threshold, to block signals -
 float thresholdR = 10;                                                         // - before the zero and max values are calculated
 
 // Filters 
BiQuadChain bqcR;                                                               // BiQuad for signal right biceps
BiQuad bq1R( 0.6844323315947305,1.368864663189461, 0.6844323315947305, 
             1.2243497755555954,0.5133795508233265); // LP
BiQuad bq2R( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306, 
            -1.2243497755555959, 0.5133795508233266); // HP
BiQuad bq3R( 0.7566897754116633, -1.2243497755555959,  0.7566897754116633,   
            -1.2243497755555959, 0.5133795508233266); // Notch
BiQuadChain bqcL;                                                               // BiQuad for signal left biceps
BiQuad bq1L( 0.6844323315947305,1.368864663189461, 0.6844323315947305,
             1.2243497755555954,0.5133795508233265); // LP
BiQuad bq2L( 0.6844323315947306, -1.3688646631894612,  0.6844323315947306, 
            -1.2243497755555959, 0.5133795508233266); // HP
BiQuad bq3L( 0.7566897754116633, -1.2243497755555959,  0.7566897754116633, 
            -1.2243497755555959, 0.5133795508233266); // Notch


void emgSample() {                                                              // EMG function
                                                             
    double emgNOFilteredR = bq3R.step(emgR.read());                          // Filtered HP value of EMG signal left biceps                
    double emgHPFilteredR = bq2R.step(emgNOFilteredR);                            // Filtered HP value of EMG signal left biceps
    double emgabsR = fabs(emgHPFilteredR);                                       // Absolute value of EMG signal left biceps          
    
    double emgNOFilteredL = bq3L.step(emgL.read());                          // Filtered HP value of EMG signal left biceps                
    double emgHPFilteredL = bq2L.step(emgNOFilteredL);                            // Filtered HP value of EMG signal left biceps
    double emgabsL = fabs(emgHPFilteredL);                                       // Absolute value of EMG signal left biceps
    
    
    
    
    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];
    }
    movmeanL = sumL/P;     
    
    
    for(int i = P-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 < P-1; n++) {                                             // Summation of array
    sumR = sumR + B[n];
    }
    movmeanR = sumR/P; //dit is de moving average waarde                        // Moving average value right biceps
    
    /* 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, movmeanL);
    //scope.set(2, emgR.read());
    //scope.set(3, movmeanR);
    /* 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 (TestButton==0 & k<200) {                                                // Loop used for gaining max and zero value
        Lvector[k] = movmeanL;
        Rvector[k] = movmeanR;
        
        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;
            for (int n = 0; n < 199; n++) {                                     
            sumY = sumY + Lvector[n];
            sumZ = sumZ + Rvector[n];
            } 
        Lwaarde[x] = sumY/200;                                                  // Value of zero for left biceps
        Rwaarde[x] = sumZ/200;                                                  // Value of zero for rigth biceps
        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);
    }
}

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
    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);
            }
    }
}