File content as of revision 4:fcada70891c5:

#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include <math.h>

MODSERIAL   pc(USBTX, USBRX);
Ticker      sampleTicker;//Ticker that measures the EMG signal every 0,002 seconds and filters it
Ticker      goTicker; //Ticker that checks if the signal threshold is reached each time a new filtered EMG signal comes in and determines if the motors are going to rotate
AnalogIn    emgl(A1);//Labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
AnalogIn    emgr(A0);
DigitalOut  ledG(LED_GREEN);
DigitalOut  ledB(LED_BLUE);
DigitalOut  ledR(LED_RED);
HIDScope    scope(2); //scope has two ports for the two EMG signals

/*coefficients of each filter
    lno = left tricep notch filter
    lhf = left tricep high pass filter
    llf = left tricep lowpass filter
    same goes for rno etc.
    */
double lno_b0 = 0.9911;     
double lno_b1 = -1.6036;   
double lno_b2 = 0.9911;    
double lno_a1 = -1.603;    
double lno_a2 = 0.9822;    

double rno_b0 = 0.9911;     
double rno_b1 = -1.6036;   
double rno_b2 = 0.9911;    
double rno_a1 = -1.603;    
double rno_a2 = 0.9822;    


double lhf_b0 = 0.9355;    
double lhf_b1 = -1.8711;   
double lhf_b2 = 0.9355;    
double lhf_a1 = -1.8669;   
double lhf_a2 = 0.8752;   

double rhf_b0 = 0.9355;    
double rhf_b1 = -1.8711;   
double rhf_b2 = 0.9355;    
double rhf_a1 = -1.8669;   
double rhf_a2 = 0.8752;    


double llf_b0 = 8.7656e-5;
double llf_b1 = 1.17531e-4;
double llf_b2 = 8.7656e-5; 
double llf_a1 = -1.9733;  
double llf_a2 = 0.9737;    

double rlf_b0 = 8.7656e-5;
double rlf_b1 = 1.17531e-4;
double rlf_b2 = 8.7656e-5; 
double rlf_a1 = -1.9733;  
double rlf_a2 = 0.9737;    


//starting values of the biquads of the corresponding filters
double lno_v1 = 0, lno_v2 = 0;
double lhf_v1 = 0, lhf_v2 = 0;
double llf_v1 = 0, llf_v2 = 0;

double rno_v1 = 0, rno_v2 = 0;
double rhf_v1 = 0, rhf_v2 = 0;
double rlf_v1 = 0, rlf_v2 = 0;

/* declaration of the outputs of each biquad.
the output of the previous biquad is the input for the next biquad.
so lno_y goes into lhf_y etc.
*/ 
double lno_y;
double lhf_y;
double llf_y;
double lrect_y;
double rno_y;
double rhf_y;
double rlf_y;
double rrect_y;

// set the threshold value for the filtered signal
//if the signal exceeds this value the motors will start to rotate 
const double threshold_value = 0.05;

/* declaration of each biquad
The coefficients will be filled in later on in void scopeSend
As said before the input of each biquad is the output of the previous one 
The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
This is done for both left and right so this makes two chains of 3 biquads */

double biquad_lno(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 biquad_lhf(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 biquad_llf(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 biquad_rno(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 biquad_rhf(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 biquad_rlf(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;
}

/* function that calculates the filtered EMG signal from the raw EMG signal. 
So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
void scopeSend(void){
    lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
    lhf_y = biquad_lhf(lno_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
    lrect_y = fabs(lhf_y);
    llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
    rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
    rhf_y = biquad_rhf(rno_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
    rrect_y = fabs(rhf_y);
    rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;  
    scope.set(1, llf_y);
    scope.set(0, rlf_y);
    scope.send();
    
    }
   
   //function that compares the filtered EMG signal to the set threshold and determines what colour the led should be
   //This led is for feedback purposes only and should obviously be replaced by the motors 
void threshold(){
  //If the right signal exceeds the threshold, the led should turn blue  
  if (rlf_y > threshold_value){
        ledB = 0;
        ledR = 1;
        ledG = 1;
        }
    //If the left signal exceeds the threshold, the led should turn red
    else if (llf_y > threshold_value){
            ledB = 1;
            ledR = 0;
            ledG = 1;
            }
    // If both signals exceed the threshold, the led should turn green         
    else if (rlf_y&&llf_y > threshold_value){
            ledB = 1;
            ledR = 1;
            ledG = 0;       
            }
    //If no signal exceeds the threshold, the led should be off
    else {
        ledB=1;
        ledG=1;
        ledR=1;
        }
    }


int main(){
    
    
   
  /*Attach the 'sample' function to the timer 'sample_timer'.
    this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
    emgSampleTicker.attach(&sample, 0.002);
    empty loop, sample() is executed periodically
    The same goes for the goTicker. It checks if the threshold is reached at the same rate new a new EMG signal comes in*/
    sampleTicker.attach(scopeSend,0.002);
    goTicker.attach(threshold,0.002);
    
    while(1) {
 
}
}