File content as of revision 32:4b73c693aefb:

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

/* DO NOT CHANGE HERE, ALREADY IMPLEMENTED IN THE CODE!!!! */

HIDScope scope( 6 );
Ticker sample_timer;


Serial pc(USBTX,USBRX);

// Inputs EMG
AnalogIn emg0_in( A0 );
AnalogIn emg1_in( A1 );
AnalogIn emg2_in( A2 );

InterruptIn but1(SW2);
InterruptIn but2(SW3);

DigitalOut led1(LED_BLUE);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_GREEN);

// Constants EMG
const double m1 = 0.5000;
const double m2 = -0.8090;
const double n0 = 0.5000;
const double n1 = -0.8090;
const double n2 = 0;
const double a1 = 0.9565;
const double a2 = -1.9131;
const double b0 = 0.9565;
const double b1 = -1.9112;
const double b2 = 0.9150;
const double c1 = 0.0675;
const double c2 = 0.1349;
const double d0 = 0.0675;
const double d1 = -1.1430;
const double d2 = 0.4128;
  
// Variables EMG
double emg0;
double emg1;
double emg2;
double notch0;
double notch1;
double notch2;
double high0;
double high1;
double high2;
double absolute0;
double absolute1;
double absolute2;
double low0;
double low1;
double low2;

// BiQuad values
BiQuadChain notch;
BiQuad N1( m1, m2, n0, n1, n2);
BiQuad N2( m1, m2, n0, n1, n2);
BiQuad N3( m1, m2, n0, n1, n2);
BiQuadChain highpass;
BiQuad H1( a1, a2, b0, b1, b2);
BiQuad H2( a1, a2, b0, b1, b2);
BiQuad H3( a1, a2, b0, b1, b2);
BiQuadChain lowpass;
BiQuad L1( c1, c2, d0, d1, d2);
BiQuad L2( c1, c2, d0, d1, d2);
BiQuad L3( c1, c2, d0, d1, d2);

const float T=0.001f;

// EMG
const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};

//Empty arrays to calculate MovAgs
double Average0, Average1, Average2; //Outcome of MovAg
const int sizeCali = 500; //Size of array over which the Threshold will be calculated
double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
//Empty arrays to calculate means in calibration

double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
int g = 0; //Part of the switch void, where the current state can be changed
int emg_calib=0; //After calibration this value will be 1, enabling the

//EMG
Ticker Filter_tick;
Ticker MovAg_tick;

//Functions  
//Filter of the first EMG signal
void filtering()
{
    emg0 = emg0_in.read(); // Reading the EMG signal
    emg1 = emg1_in.read();
    emg2 = emg2_in.read(); 
    notch0 = N1.step(emg0); // Applying a notch filter over the EMG data
    notch1 = N2.step(emg1);
    notch2 = N3.step(emg2); 
    high0 = H1.step(notch0); // Applying a high pass filter
    high1 = H2.step(notch1); 
    high2 = H3.step(notch2);
    absolute0 = fabs(high0); // Rectifying the signal
    absolute1 = fabs(high1);
    absolute2 = fabs(high2); 
    low0 = L1.step(absolute0); // Applying low pass filter
    low1 = L2.step(absolute1);
    low2 = L3.step(absolute2); 
}

void MovAg()
{    
    for (int i = sizeMovAg-1; i>=0; i--) 
        {
        //For statement to make an array of the last datapoints of the filtered signal
        StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
        }
    
    StoreArray0[0] = low0; //Stores the latest datapoint in the first element of the array
    StoreArray1[0] = low1;
    StoreArray2[0] = low2;
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;

    for (int a = 0; a<=sizeMovAg-1; a++) 
        { //For statement to sum the elements in the array
        sum1+=StoreArray0[a];
        sum2+=StoreArray1[a];
        sum3+=StoreArray2[a];
        }
        
    Average0 = sum1/sizeMovAg; //Calculates an average over the datapoints in the array
    Average1 = sum2/sizeMovAg;
    Average2 = sum3/sizeMovAg;
    
    scope.set( 0, emg0); // Sending the signal to the HIDScope 
    scope.set( 1, low0); // Change the numer of inputs on the top when necessary
    scope.set( 2, Average0);
    scope.set( 3, low1);
    scope.set( 4, emg2);
    scope.set( 5, low2);
    scope.send(); 
}

void switch_to_calibrate() //Void to switch between signals to calibrate
{
    g++;
    if (g == 0) 
    { //If g = 0, led is blue
        led1=0;
        led2=1;
        led3=1;
    } 
    else if(g == 1) 
    { //If g = 1, led is red
        led1=1;
        led2=0;
        led3=1;

    } 
    else if(g == 2) 
    { //If g = 2, led is green
        led1=1;
        led2=1;
        led3=0;
    } 
    else 
    { //If g > 3, led is white
        led1=0;
        led2=0;
        led3=0;
        emg_calib = 0;
        g = 0;
    }
}

void calibrate(void) //Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
{
    switch (g) 
    {
        case 0: 
        { //Case zero, calibrate EMG signal of right biceps
            sum = 0.0;
            for (int j = 0; j<=sizeCali-1; j++) 
            {
                //For statement to make an array of the latest datapoints of the filtered signal
                StoreCali0[j] = low0; //Stores the latest datapoint in the first element of the array
                sum+=StoreCali0[j]; //Sums the elements in the array
                wait(0.001f);
            }
            Mean0 = sum/sizeCali; //Calculates the mean of the signal
            Threshold0 = Mean0; //Factor *2 is for resting and *1 is for max contraction
            break;
        }
        case 1: 
        { //Case one, calibrate EMG signal of left biceps
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali1[j] = low1;
                sum+=StoreCali1[j];
                wait(0.001f);
            }
            Mean1 = sum/sizeCali;
            Threshold1 = Mean1; //Factor *2 is for resting and *1 is for max contraction
            break;
        }
        case 2: 
        { //Case two, calibrate EMG signal of calf
            sum = 0.0;
            for(int j=0; j<=sizeCali-1; j++) 
            {
                StoreCali2[j] = low2;
                sum+=StoreCali2[j];
                wait(0.001f);
            }   
            Mean2 = sum/sizeCali;
            Threshold2 = Mean2; //Factor *2 is for resting and *1 is for max contraction
            break;
        }
        case 3: 
        { //Sets calibration value to 1; robot can be set to Home position
            emg_calib=1;
            wait(0.001f);
            break;
        }
        default: 
        { //Ensures nothing happens if g is not equal to 0,1 or 2.
        break;
        }
    }
}

void big_calibration_function()
{
    for(int m = 1; m <= 4; m++)
    {
        led1 = 0;
        led2 = 1;
        led3 = 1; 
        
        MovAg_tick.attach(&MovAg,T);                                //Moving average calculation every T sec.
        
        
        pc.printf("g is %i\n\r",g);
        pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
        pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
        
        bool k = true;
        while(k == true)
        {
            if(but2 == false)
            {
                calibrate();                                //Calibrate threshold for 3 muscles
                k = false;
            }
            wait(0.2f);                                             //Wait to avoid bouncing of button
        }
        
        bool h = true;
        while(h == true)
        {
            if (but1 == false)
            {
                switch_to_calibrate();                      //Switch state of calibration (which muscle)
                h = false;
            }
            
            wait(0.2f);
        }                                             //Wait to avoid bouncing of button
        
        //wait(2.0f);   
        }
}


int main() 
{  
    Filter_tick.attach(&filtering,T);                         //EMG signals filtered every T sec.
    pc.baud(115200); // setting baudrate 
    
    big_calibration_function();
}