main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "math.h"
00005 
00006 
00007 Serial pc(USBTX, USBRX);
00008 
00009 
00010 //Defining all in- and outputs
00011 //EMG input
00012 AnalogIn    emgBR( A0 );    //Right Biceps
00013 AnalogIn    emgBL( A1 );    //Left Biceps
00014 
00015 //LED output
00016 DigitalOut  led_B( LED_BLUE );
00017 DigitalOut  led_R( LED_RED );
00018 DigitalOut  led_G( LED_GREEN );
00019 
00020 //Setting Tickers for sampling EMG and determing if the threshold is met
00021 Ticker      sample_timer;
00022 Ticker      threshold_timer;
00023 
00024 // Defining HIDScope ports needed
00025 HIDScope    scope( 2 );
00026 
00027 
00028 /* Defining all the different BiQuad filters, which contain a Notch filter,
00029 High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
00030 between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
00031 and the Low-pass filter cancels out all frequencies below 4 Hz */
00032 
00033 /* Defining all the normalized values of b and a in the Notch filter for the
00034 creation of the Notch BiQuad */
00035 
00036 BiQuad      bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00037 BiQuad      bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
00038 
00039 /* Defining all the normalized values of b and a in the High-pass filter for the
00040 creation of the High-pass BiQuad */
00041 
00042 BiQuad      bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00043 BiQuad      bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
00044 
00045 /* Defining all the normalized values of b and a in the Low-pass filter for the
00046 creation of the Low-pass BiQuad */
00047 
00048 BiQuad      bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00049 BiQuad      bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
00050 
00051 // Creating a variable needed for the creation of the BiQuadChain
00052 BiQuadChain bqChain1;
00053 BiQuadChain bqChain2;
00054 
00055 
00056 //Defining all doubles needed in the filtering process
00057 double emgBRfiltered;   //Right biceps Notch+High pass filter
00058 double emgBRrectified;  //Right biceps rectified
00059 double emgBRcomplete;   //Right biceps low-pass filter, filtering complete
00060 
00061 double emgBLfiltered;   //Left biceps Notch+High pass filter
00062 double emgBLrectified;  //Left biceps rectified
00063 double emgBLcomplete;   //Left biceps low-pass filter, filtering complete
00064 
00065 
00066 double threshold = 0.03;
00067 
00068 
00069 /** Sample function
00070  * this function samples the BR EMG, filters the EMG and sends it to HIDScope
00071  **/
00072 void EMG_sample()
00073 {
00074     emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
00075     emgBRrectified = fabs(emgBRfiltered);            //Rectification
00076     emgBRcomplete = bqLow1.step(emgBRrectified);     //Low-pass
00077     
00078     
00079    
00080     emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
00081     emgBLrectified = fabs( emgBLfiltered );           //Rectification
00082     emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
00083     
00084     /* Set the sampled emg values in channel 0 (the first channel) and 1 
00085     (the second channel) in the 'HIDScope' instance named 'scope' */
00086     
00087     scope.set(1, emgBLcomplete );
00088     scope.set(0, emgBRcomplete );
00089     
00090        
00091     /*  Finally, send all channels to the PC at once */
00092     scope.send();
00093     /* To indicate that the function is working, the LED is toggled */
00094     led_B = !led_B;
00095     
00096     
00097 }
00098 
00099 double numsamples = 500;
00100 double emgBRarray[500];
00101 double emgBRsum;
00102 double emgBRmeanMVC;
00103 double thresholdBR;
00104 
00105 double getThresholdBR()
00106 {
00107     for (int i=0; i<numsamples; i++) {
00108         emgBRarray[i] = emgBRcomplete;
00109         emgBRsum = emgBRsum + emgBRarray[i];
00110         }
00111     
00112     emgBRmeanMVC = emgBRsum / numsamples;
00113     
00114     thresholdBR = emgBRmeanMVC * 0.3;
00115     return thresholdBR;
00116 }
00117     
00118 double getThresholdBL()
00119 {
00120     double numsamples = 500;
00121     double emgBLarray[500];
00122     double emgBLsum;
00123     for (int i=0; i<numsamples; i++) {
00124         emgBLarray[i] = emgBLcomplete;
00125         emgBLsum = emgBLsum + emgBLarray[i];
00126         }
00127     
00128     double emgBLmeanMVC = emgBLsum / numsamples;
00129     
00130     double thresholdBL = emgBLmeanMVC * 0.3;
00131     return thresholdBL;
00132 }
00133     
00134 
00135 // Function to make the BiQuadChain for the Notch and High pass filter for all three filters    
00136 void getbqChain()
00137 {
00138     bqChain1.add(&bqNotch1).add(&bqHigh1);                 //Making the BiQuadChain
00139     bqChain2.add(&bqNotch2).add(&bqHigh2);
00140 }
00141 
00142 
00143 // Function to check if the threshold is met, with LED feedback for the user
00144 void ThresholdReached()
00145 {
00146     if (emgBRcomplete > thresholdBR) 
00147     {
00148         led_G = 0;
00149         led_R = 1;
00150         led_B = 1;
00151     }
00152     
00153     else if (emgBLcomplete > threshold)
00154     {
00155         led_G = 1;
00156         led_R = 0;
00157         led_B = 1;
00158     }
00159         
00160         
00161     else {
00162         led_G = 1;
00163         led_R = 1;
00164         led_B = 0; 
00165         }
00166         
00167 }
00168   
00169 
00170 int main()
00171 {   
00172     /*Attach the 'sample' function to the timer 'sample_timer'.
00173     this ensures that 'sample' is executed every 0.002 seconds, to get a 
00174     sample frequency of 500 Hz
00175     */
00176     pc.baud(115200);
00177     getbqChain();
00178     sample_timer.attach(&EMG_sample, 0.002);
00179     thresholdBR = getThresholdBR();
00180     pc.printf("Threshold is %f.2\n", threshold);
00181     threshold_timer.attach(&ThresholdReached, 0.002);
00182             
00183     
00184     while(1) {}
00185 }