File content as of revision 32:00c6e3502bd9:

#include "mbed.h"
#include "HIDScope.h"
#define MOV_AVG_NUM     100
#include "math.h"

//Define objects
AnalogIn    b_emg(A0); //Analog input bicep
AnalogIn    t_emg(A1); //Analog input tricep
AnalogIn    p_emg(A2); //Analog input bicep for push
//HIDScope    scope_hid(6);

double b_highV[4];
double b_lowV[4];
double t_highV[4];
double t_lowV[4];
double p_highV[4];
double p_lowV[4];
double b_min = 10.0f;
double t_min = 10.0f;
double p_min = 10.0f;


double filter(double input, double coeff_input[], double coeff_output[], double prev_outputs[])
{
    double new_input = input;
    for(int i=1; i<5; i++)
        new_input -= coeff_input[i] * prev_outputs[i-1];

    double new_output = coeff_output[0] * new_input;
    for(int i=1; i<5; i++)
        new_output += coeff_output[i] * prev_outputs[i-1];

    // Set the new output as the first value of the 'recent outputs'
    for(int i = 3; i > 0; i--)
        prev_outputs[i] = prev_outputs[i-1];
    prev_outputs[0] = new_input;
    return new_output;
}
double fh_b[]= {0.7602, -3.0406, 4.5609, -3.0406, 0.7602};
double fh_a[]= {1, -3.4532, 4.5041, -2.6273, 0.5778};
double b_highpass_filter(double u)
{
    return filter(u, fh_a, fh_b, t_highV);
}
double t_highpass_filter(double u)
{
    return filter(u, fh_a, fh_b, b_highV);
}
double p_highpass_filter(double u)
{
    return filter(u, fh_a, fh_b, p_highV);
}

double fl_b[]= {0.00001329,    0.00005317,    0.00007976,   0.00005317,    0.00001329};
double fl_a[]= {1.0000,   -3.6717,    5.0680,   -3.1160,    0.7199};
double b_lowpass_filter(double u)
{
    return filter(u, fl_a, fl_b, t_lowV);
}
double t_lowpass_filter(double u)
{

    return filter(u, fl_a, fl_b, b_lowV);
}
double p_lowpass_filter(double u)
{

    return filter(u, fl_a, fl_b, p_lowV);
}

double rust;
void emg_cal(int emg)
{
    double mem[300];
    double cur;
    double mean;
    if(emg == 0) {
        for(int i=0; i<900; i++) {
            cur = b_emg.read();
            double output1 = b_highpass_filter(cur);
            double output2 = fabs(output1);
            cur = b_lowpass_filter(output2);
            if(i >= 300 && i < 600) {
                mem[i-300] = cur;
                mean += cur;
            }
            wait(0.002);
        }
        mean /= 300;
        double variance = 0;
        for (int i = 0; i < 300; ++i) {
            variance += pow((mem[i] - mean),2);
        }
        variance /=300;
        rust =  mean-variance;
        printf("Rust %f\r\n",rust); 
    }
    
    if(emg == 1) {
        for(int i=0; i<900; i++) {
            cur = b_emg.read();
            double output1 = b_highpass_filter(cur);
            double output2 = fabs(output1);
            cur = b_lowpass_filter(output2);
            if(i >= 300 && i < 600) {
                mem[i-300] = cur;
                mean += cur;
            }
            wait(0.002);
        }
        mean /= 300;
        double variance = 0;
        for (int i = 0; i < 300; ++i) {
            variance += pow((mem[i] - mean),2);
        }
        variance /=300;
        b_min =  mean-variance;
        printf("b %f\r\n",b_min);
        printf("r %f\r\n",rust);
        b_min += rust;
        b_min /= 2;        
        printf("b_min %f\r\n",b_min);
    }
    else if(emg == 2) {
        for(int i=0; i<900; i++) {
            cur = t_emg.read();
            double output1 = t_highpass_filter(cur);
            double output2 = fabs(output1);
            cur = t_lowpass_filter(output2);
            if(i >= 300 && i < 600) {
                mem[i-300] = cur;
                mean += cur;
            }
            wait(0.002);
        }
        mean /= 300;
        double variance = 0;
        for (int i = 0; i < 300; ++i) {
            variance += pow((mem[i] - mean),2);
        }
        variance /=300;
        t_min =  mean-variance;
        printf("b %f\r\n",t_min);
        printf("r %f\r\n",rust);
        t_min += rust;
        t_min /= 2;
        printf("t_min %f\r\n",t_min);
    }
    
    else if(emg == 3) {
        for(int i=0; i<900; i++) {
            cur = p_emg.read();
            double output1 = p_highpass_filter(cur);
            double output2 = fabs(output1);
            cur = p_lowpass_filter(output2);
            if(i >= 300 && i < 600) {
                mem[i-300] = cur;
                mean += cur;
            }
            wait(0.002);
        }
        mean /= 300;
        double variance = 0;
        for (int i = 0; i < 300; ++i) {
            variance += pow((mem[i] - mean),2);
        }
        variance /=300;
        p_min =  mean-variance;
        printf("b %f\r\n",p_min);
        printf("r %f\r\n",rust);
        p_min += rust;
        p_min /= 2;
        printf("p_min %f\r\n",p_min);
    }
    
    
}


/** Sample function
 * this function samples the emg and sends it to HIDScope
 **/
void sample(double& bicout, double& tricout, double& pushout)
{
    double last_biceps = 0;
    double last_triceps = 0;
    double last_push = 0;

    double b_input = b_emg.read();
    double output1 = b_highpass_filter(b_input);
    double output2 = fabs(output1);
    double output3 = b_lowpass_filter(output2);

    double t_input = t_emg.read();
    double output4 = t_highpass_filter(t_input);
    double output5 = fabs(output4);
    double output6 = t_lowpass_filter(output5);

    double p_input = p_emg.read();
    double output7 = p_highpass_filter(p_input);
    double output8 = fabs(output7);
    double output9 = p_lowpass_filter(output8);

    /*double tot = output3;
    for(int i=0; i<MOV_AVG_NUM-1; i++) {
        tot += lastOutputs[i];
        if(i != 0) lastOutputs[i] = lastOutputs[i-1];
    }
    lastOutputs[0] = output3;
    output3 = tot/MOV_AVG_NUM;*/

    /* Second, set the sampled emg value in channel zero (the first channel) in the 'HIDScope' variable named 'scope' */

    if (last_biceps == 0 and output3 > b_min+b_min*0.1) {
        bicout = 1;
    } else if (last_biceps == 1 and output3 < b_min-b_min*0.1) {
        bicout = 0;
    } else {
        bicout = last_biceps;
    }
    //printf("minbic %f\t",bicout);
    last_biceps = bicout;

    /*if (output3>0.06) {//This is the output for the right bicep
        bicout=1;
    } else if(output3>0.04) {
        bicout=0.5;
    } else {
        bicout=0;
    }*/

    /*if (output6>0.0561) {//this is the output for the right tricep
        tricout=1;
    } else if(output6>0.0380) {
        tricout=0.5;
    } else {
        tricout=0;
    }*/

    if (last_triceps == 0 and output6 > t_min+0.1*t_min) {
        tricout = 1;
    } else if(last_triceps == 1 and output6 < t_min-0.1*t_min) {
        tricout = 0;
    } else {
        tricout = last_triceps;
    }
    //printf("mintric %f\t",tricout);
    last_triceps = tricout;

    if (last_push ==0 and output9>p_min+0.1*p_min) {//this is the output for the left bicep (the push motion)
        pushout=1;
    } else if(last_push == 1 and output9<p_min-0.1*p_min) {
        pushout=0;
    } else {
        pushout = last_push;
    }
    //printf("minpush %f\r\n",pushout);

    last_push = pushout;

    /*scope_hid.set(0,output3);
    scope_hid.set(1,output6);
    scope_hid.set(2,output9);
    scope_hid.set(3,bicout);
    scope_hid.set(4,tricout);
    scope_hid.set(5,pushout);

    scope_hid.send();*/
}