File content as of revision 2:ac98d055a6cd:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
AnalogIn    emg_left(A0);
AnalogIn emg_right(A1);
DigitalOut led(LED_BLUE);
HIDScope scope(6);
Ticker get;
AnalogIn pot(A1);
Serial pc(USBTX, USBRX); // tx, rx
volatile bool fn_go = false;
double y_notch_1;
double y_notch_2;
double y_high;
double y_low;
double y_rect;
const double Fs = 500*2;
const double Ts = 1/Fs;
double a;
const double gain_notch_1 = 1.000000;
const double b0_notch_1 = 1.0*gain_notch_1;
const double b1_notch_1 = -1.90215058158*gain_notch_1;
const double b2_notch_1 = 1.0*gain_notch_1;
const double a1_notch_1 = -1.76851639902;
const double a2_notch_1 =  0.89458928499;
double v1_notch_1 = 0, v2_notch_1 = 0;


const double gain_notch_2 = 1.000000;
const double b0_notch_2 = 1.0*gain_notch_2;
const double b1_notch_2 = -1.90215058158*gain_notch_2;
const double b2_notch_2 = 1.0*gain_notch_2;
const double a1_notch_2 = -1.85196790295;
const double a2_notch_2 =  0.92089778695;
double v1_notch_2 = 0, v2_notch_2 = 0;
//high pass of 5 Hz Wc

const double gain_high = 1;
const double b0_high =   0.978030479206560*gain_high;
const double b1_high = -1.956060958413119*gain_high;
const double b2_high = 0.978030479206560*gain_high;
const double a1_high =  -1.955578240315036;
const double a2_high =   0.956543676511203;
double v1_high = 0, v2_high = 0;
double v1_high_1 = 0, v2_high_1=0;

//low pass of 0.3 Hz Wc
const double gain_low = 0.00001;
const double b0_low =   0.088708177364838*gain_low;
const double b1_low =  0.177416354729676*gain_low;
const double b2_low = 0.088708177364838*gain_low;
const double a1_low =  -1.997334271812535;
const double a2_low =    0.997337820139629;
double v1_low = 0, v2_low = 0;
double v1_low_1 = 0,v2_low_1=0;
double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

double biquad1(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

double biquad2(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

double biquad3(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}

double biquad4(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2)
{
    double v = u - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2 = v1;
    v1 = v;
    return y;
}




void fn_activate()
{

    fn_go = true;
}

double z1;
double y_high_1;
double y_notch_1_1;
double y_notch_2_1;
double y_rect_1;
double y_low_1;
double t = 0;
double value = 0;
double max() // calibration
{
    if (t <= 10) {
        value = value + 0.0;
    } else if(t>10 && t<=20) {
        if(value<=y_low) {
            value = y_low;

        } else {
            value = value +0.0;
        }

    } else {
        value = value + 0.0;


    }
    return value*0.80;
}
double val;
double gain()
{

    if(max()==0) {
        val = 0;
    } else if(max()>=0) {
        val = 1/max();
    }
    return val;
}
const double a1 = 11.1111;
const double b1 = -0.1111;
const double a2 = 4.7619;
const double b2 = -0.047619;
void scopeSend()
{
    y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high);
    y_rect = abs(y_high);
    y_low = biquad2(y_rect,  v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low);
    t = t+Ts;
    y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high);
     y_rect_1 = abs(y_high_1);
     y_low_1 =   biquad4(y_rect_1,  v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low);

    scope.set(0,emg_left.read());
    scope.set(1,emg_right.read());
    scope.set(2,y_low);
    scope.set(3,y_low_1);
    scope.set(4,a1*y_low + b1);
    scope.set(5,a2*y_low_1 + b2);


    scope.send();
}

int main()
{
    get.attach(&fn_activate,Ts);

    while (true) {
        if(t<=10) {
            led = 0;
        }
        if(t>10 && t<=20)
        {
            led = 1;
        }
        if(t>20)
        {
         led = 0;   
        }
        if(fn_go == true) {

            scopeSend();


            fn_go = false;
        }
    }
}