File content as of revision 28:fc099a3e37fd:

#include "mbed.h"
#include "HIDScope.h"
#include "BiQuad.h"
#include "MODSERIAL.h"

#include "FastPWM.h"  
#include "QEI.h"
float pwmprocent1 = 15;             // introduce duty cycle variable translational motor
float pwmprocent2 = 15;             // introduce duty cycle for rotational motor (SLOW DOWN TO PREVENT OVERSHOOT?)
int modesw;                 // introduce mode to switch from translational to rotational control
FastPWM motorpwm1(D5);       // define PWM Pin
FastPWM motorpwm2(D6);      // define motor 2 pwm pin
float totalpwm1 = pwmprocent1/100;
float totalpwm2 = pwmprocent2/100;
int upperlimit = 1200; 
int lowerlimit = 3920;
DigitalOut m1direction(D4);     // direction translational motor
DigitalOut m2direction(D7);     // direction

//Define objects
    AnalogIn    emg1_in( A0 ); /* read out the signal */
    AnalogIn    emg2_in( A1 );
    AnalogIn    emg3_in( A2 );
    DigitalIn   max_reader1( SW2 );
    DigitalIn   max_reader3( SW3 );

    Ticker      main_timer;
    Ticker      max_read1;
    Ticker      max_read3;
    HIDScope    scope( 5 );
    DigitalOut  red(LED_RED);
    DigitalOut  blue(LED_BLUE);
    DigitalOut  green(LED_GREEN);
    MODSERIAL   pc(USBTX, USBRX);


// EMG variables
    //Right Biceps
    double emg1;
    double emg1highfilter;
    double emg1notchfilter;
    double emg1abs;
    double emg1lowfilter;
    double emg1peak;
    double maxpart1;
    // Left Biceps
    double emg2;
    double emg2highfilter;
    double emg2notchfilter;
    double emg2abs;
    double emg2lowfilter;
    double emg2peak;
    double max1;
    double maxpart2;
    // Left Lower Arm OR Triceps
    double emg3;
    double emg3highfilter;
    double emg3notchfilter;
    double emg3abs;
    double emg3lowfilter;
    double emg3peak;
    double max3;
    double maxpart3;

// BiQuad Filter Settings
    // Right Biceps
    BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
    BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
    BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
    BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
    // Left Biceps
    BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
    BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
    BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
    BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
    // Left Lower Arm OR Triceps
    BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
    BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
    BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
    BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
    //

// stopfunction
void superstop()
{
    int stoptimes;
    if (modesw==0)
    {
         motorpwm1.write(0);
         for (int a = 0; a < pwmprocent1; a = a + 1)
         {
             if (m1direction = 0)
             {
                 m1direction = 1;
             }
             else
             {
                 m1direction = 0;
             }
         }
     else
     {
         motorpwm2.write(0);
         for (int a = 0; a < pwmprocent2; a = a+1)
         {
             if (m2direction = 0)
             {
                 m2direction = 1;
             }
             else
             {
                 m2direction = 0;
             }
         }
     } 
}

// Finding max values for correct motor switch if the button is pressed
void get_max1(){
    if (max_reader1==0){
            green = !green;
            red = 1;
            blue = 1;
            
            for(int n=0;n<2000;n++){  /* measure 2000 samples and filter it */
            emg1 = emg1_in.read();      /* read out emg */
            emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
            emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
            emg1abs = fabs(emg1notchfilter); /* take the absolute value */
            emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
            emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
            
            if (max1<emg1peak){
                max1 = emg1peak; /* set the max value at the highest measured value */
            }
            wait(0.001f); /* measure at 1000Hz */   
            }
            wait(0.2f);
            green = 1;
    }
    maxpart1 = 0.25*max1; /* set cut off voltage at 25% of max for right biceps */
    maxpart2 = 0.15*max1; /* set cut off votage at 15% of max for left biceps */
}

void get_max3(){
    if (max_reader3==0){
            green = 1;
            blue = 1;
            red = !red;
            for(int n=0;n<2000;n++){
                        
            emg3 = emg3_in.read();
            emg3highfilter = filterhigh3.step(emg3);
            emg3notchfilter = filternotch3.step(emg3highfilter);
            emg3abs = fabs(emg3notchfilter);
            emg3lowfilter = filterlow3.step(emg3abs);
            emg3peak = filterpeak3.step(emg3lowfilter);
            
            if (max3<emg3peak){
                max3 = emg3peak; /* set the max value at the highest measured value */
            }
            wait(0.001f);    
            }
            wait(0.2f);
            red = 1;
    }
    maxpart3 = 0.25*max3; /* set cut off voltage at 25% of max for right biceps */
}

// Filtering & Scope
void filter() {
    // Right Biceps
    emg1 = emg1_in.read();
    emg1highfilter = filterhigh1.step(emg1);
    emg1notchfilter = filternotch1.step(emg1highfilter);
    emg1abs = fabs(emg1notchfilter);
    emg1lowfilter = filterlow1.step(emg1abs);
    emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
    // Left Biceps
    emg2 = emg2_in.read();
    emg2highfilter = filterhigh2.step(emg2);
    emg2notchfilter = filternotch2.step(emg2highfilter);
    emg2abs = fabs(emg2notchfilter);
    emg2lowfilter = filterlow2.step(emg2abs);
    emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
    // Left Lower Arm OR Triceps
    emg3 = emg3_in.read();
    emg3highfilter = filterhigh3.step(emg3);
    emg3notchfilter = filternotch3.step(emg3highfilter);
    emg3abs = fabs(emg3notchfilter);
    emg3lowfilter = filterlow3.step(emg3abs);
    emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
    int count;
    QEI Encoder(D12,D13,NC,64);
    /* Compare measurement to the calibrated value to decide actions */
    if (maxpart1<emg1peak){ /* See if right biceps is contracting */
        red = 0;
        blue = 1;
        green = 1;
        
        switch (modesw)
        {
            case 0:
            {
                modesw = 1;
                wait(1.0);
                break;
            }
            case 1:
            {
                modesw = 0;
                wait(1.0);
                break;
            }

        }
    else {
        if (maxpart2<emg2peak){ /* See if left biceps is contracting */
            red = 1;
            blue = 0;
            green = 1;
   
                switch (modesw)
                {
                    case 0:
                    {
                        m1direction = 1;
                        motorpwm1.write(totalpwm1);
                        break;
                    }
                    case 1:
                    {
                        count = Encoder.getPulses();
                        m2direction = 1;
                        if (count<lowerlimit)
                        {
                            motorpwm2.write(totalpwm2);
                        break;
                        }
                        else
                        {
                            superstop();
                    }
                }
                
            }
        
    else {
        if (maxpart3<emg3peak){ /* See if lower arm is contracting */
            red = 1;
            blue = 1;
            green = 0;
                        
            switch (modesw)
            {
                case 0:
                {
                    m1direction = 0;
                    motorpwm1.write(totalpwm1);
                    break;
                }
                case 1:
                {             
                    If (count>higherlimit)
                    {
                        m1direction = 0;
                        motorpwm2.write(totalpwm1);
                        break;
                    }
                    else
                    superstop();
                }
            }
        
    else {
        red = 1; /* Shut down all led colors if no movement is registered */
        blue = 1;
        green = 1;
        }
        }
        }
    /* 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, emg1peak ); /* plot Right biceps voltage */
    scope.set(1, emg2peak ); /* Plot Left biceps voltage */
    scope.set(2, maxpart1 ); /* Show the line above which the motor should run for right biceps */
    scope.set(3, emg3peak ); /* Plot Lower Arm voltage */
    scope.set(4, maxpart3 ); /* Plot the line above which the motor should run for lower arm */
 
    scope.send(); /* send everything to the HID scope */
}

int main(){   

    main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
    max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
    max_read3.attach(&get_max3, 2);
    while(1) {}
}