File content as of revision 5:e7253884c2e4:

#include "mbed.h"
#include "math.h"
#include "BiQuad.h"
#include "Servo.h"
#include "QEI.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(D8);
DigitalOut motor1direction(D7);
PwmOut motor1control(D6);
DigitalOut motor2direction(D4);
PwmOut motor2control(D5);
InterruptIn button1(D10);
InterruptIn button2(D11);

QEI EncoderL(D12,D13,NC,64,QEI::X2_ENCODING);
QEI EncoderR(D2,D3,NC,64,QEI::X2_ENCODING);

Ticker PrintTicker;
Serial pc(USBTX, USBRX);


 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;                                                         //-------
 const bool clockwise1 = true;
 const bool clockwise2 = true;
 volatile bool direction1 = clockwise1;
 volatile bool direction2 = clockwise2;
 
 //------------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);
    }   
}


//--------------------------Directions motor 1 aanpassen-------------------------
void onButtonPress1() {
    // reverses the direction
    motor1direction.write(direction1= !direction1);
    pc.printf("direction1: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
}

//--------------------------Directions motor 2 aanpassen-------------------------
void onButtonPress2() {
    // reverses the direction
    motor2direction.write(direction2= !direction2);
    pc.printf("direction2: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
}

//-----------------------Print functie----------------------------------------------
void Printen() {
    
    const float pi = 3.141592653589793;                                             // Value of pi
    double gearratio = 3.857142857;
    double radiuspulley = 15.915;
    double hoekgraad = EncoderL.getPulses() * 0.0857142857;
    double hoekrad = hoekgraad * 0.0174532925;
    double hoekgraad2 = EncoderR.getPulses() * 0.0857142857;
    double hoekrad2 = hoekgraad2 * 0.0174532925;
    double hoekarm = hoekgraad2 / gearratio;
    double translatie = hoekgraad /360 * 2 * pi * radiuspulley;
    

    pc.printf("counts :%i \r\n", EncoderL.getPulses());
    pc.printf("hoekgraad :%f \r\n", hoekgraad );
    pc.printf("hoekrad :%f \r\n", hoekrad );
    pc.printf("translatie :%f mm \r\n", translatie );
    pc.printf("\n");
    
    
    pc.printf("counts2 :%i \r\n", EncoderR.getPulses());
    pc.printf("hoekgraad2 :%f \r\n", hoekgraad2 );
    pc.printf("hoekrad2 :%f \r\n", hoekrad2 );
    pc.printf("hoekarm :%f \r\n", hoekarm );
    pc.printf("\n");
    
}

int main()
{  
//------------------EMG samplen-----------------------------
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;

//-----------------Motor button directions----------------------------
pc.baud(115200);
    
    button1.fall(&onButtonPress1);
    button2.fall(&onButtonPress2);
    PrintTicker.attach(&Printen, 1);


//------------------Oneindige loop--------------------------------------
    while(1) {
        
        if (movagL > thresholdL)
        {   motor1control.write(0.8);
            motor1direction.write(true);
            }
        else {
            motor1control.write(0);
        }
            
        if (movagR > thresholdR)
        {   motor2control.write(0.8);
            motor2direction.write(true);
            }
        else {
            motor1control.write(0);
        }
            
            
        if (movagS > thresholdS)
        {   myservo = 0.5;
            wait(0.01);
        }
        else {
            myservo = 0.0;
            wait(0.01);
        }
            

    }
}