EMG.h

00001 #include "biquadFilter.h"
00002 #include <cmath>
00003  
00004  
00005 const int Fs = 200; // sampling frequency
00006 const double low_b1 = 0.000944691843840; //filter coefficients - second order butterworth filters at 2 hz low and 25 hz high, coefficents based on Fs of 512.
00007 const double low_b2 = 0.001889383687680;
00008 const double low_b3 = 0.000944691843840;
00009 const double low_a2 = -1.911197067426073; // a1 is normalized to 1
00010 const double low_a3 = 0.914975834801434;
00011 const double high_b1 = 0.569035593728849;
00012 const double high_b2 = -1.138071187457699;
00013 const double high_b3 = 0.569035593728849;
00014 const double high_a2 = -0.942809041582063; // a1 is normalized to 1
00015 const double high_a3 = 0.333333333333333;
00016 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3); // different objects for different inputs, otherwise the v1 and v2 variables get fucked up
00017 biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
00018 biquadFilter highpass3(high_a2, high_a3, high_b1, high_b2, high_b3);
00019 biquadFilter highpass4(high_a2, high_a3, high_b1, high_b2, high_b3);
00020 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
00021 biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
00022 biquadFilter lowpass3(low_a2, low_a3, low_b1, low_b2, low_b3);
00023 biquadFilter lowpass4(low_a2, low_a3, low_b1, low_b2, low_b3);
00024  
00025 AnalogIn input1(A0); // declaring the 4 inputs
00026 AnalogIn input2(A1);
00027 AnalogIn input3(A2);
00028 AnalogIn input4(A3);
00029 double u1; double y1; // declaring the input variables
00030 double u2; double y2;
00031 double u3; double y3;
00032 double u4; double y4;
00033 
00034 Ticker T1;
00035 volatile bool sample_go;
00036 volatile bool calibrate_go;
00037 
00038 InterruptIn cali_button(PTA4); // initialize interrupt button for calibration stuff
00039 double cali_fact1 = 8;
00040 double cali_fact2 = 8; // calibration factor to normalize filter output to a scale of 0 - 1
00041 double cali_fact3 = 8;
00042 double cali_fact4 = 8;
00043 
00044 void sample_filter()
00045 {
00046     u1 = input1; u2 = input2; u3 = input3; u4 = input4; // sample
00047     y1 = highpass1.step(u1); y2 = highpass2.step(u2); y3 = highpass3.step(u3); y4 = highpass4.step(u4); // filter order is: high-pass --> rectify --> low-pass
00048     y1 = fabs(y1); y2 = fabs(y2); y3 = fabs(y3); y4 = fabs(y4);
00049     y1 = lowpass1.step(y1)*cali_fact1; y2 = lowpass2.step(y2)*cali_fact2; y3 = lowpass3.step(y3)*cali_fact3; y4 = lowpass4.step(y4)*cali_fact4; // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output of dennis.
00050 }
00051 
00052 
00053 void calibratego()
00054 {
00055     calibrate_go = 1;
00056 }
00057 
00058 void samplego() // ticker function, set flag to true every sample interval
00059 {
00060     sample_go = 1;
00061 }
00062 
00063 void calibrate() // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
00064 {
00065     statusled=0;
00066     cali_fact1 = 1; cali_fact2 = 1; cali_fact3 = 1; cali_fact4 = 1; 
00067     double cali_max1 = 0; double cali_max2 = 0; double cali_max3 = 0; double cali_max4 = 0;
00068     lcd.cls(); 
00069     lcd.printf("calibrating...\nFlex muscle Y1");
00070     for(int i = 0; i < 5*Fs; i++)
00071     {
00072         sample_filter();
00073         if(y1 > cali_max1)
00074         {
00075             cali_max1 = y1;
00076         }
00077         wait(1/(float)Fs);
00078    }
00079    statusled=1;
00080    wait(1); // seperated calibration of muscles, including a wait period of a second between the calibration of each thing.
00081    statusled=0;
00082    lcd.cls(); 
00083    lcd.printf("calibrating...\nFlex muscle Y2");
00084    for(int i = 0; i < 5*Fs; i++)
00085     {
00086         sample_filter();
00087         if(y2 > cali_max2)
00088         {
00089             cali_max2 = y2;
00090         }
00091         wait(1/(float)Fs);
00092    }
00093    statusled=1;
00094    wait(1);
00095    statusled=0;
00096    lcd.cls(); 
00097    lcd.printf("calibrating...\nFlex muscle Y3");
00098     for(int i = 0; i < 5*Fs; i++)
00099     {
00100         sample_filter();
00101         if(y3 > cali_max3)
00102         {
00103             cali_max3 = y3;
00104         }
00105         wait(1/(float)Fs);
00106     }
00107     statusled=1;
00108     wait(1);
00109     statusled=0;
00110     lcd.cls(); 
00111     lcd.printf("calibrating...\nFlex muscle Y4");
00112     for(int i = 0; i < 5*Fs; i++)
00113     {
00114         sample_filter();
00115         if(y4 > cali_max4)
00116         {
00117             cali_max4 = y4;
00118         }
00119         wait(1/(float)Fs);
00120     }
00121    cali_fact1 = 1.0/cali_max1;
00122    cali_fact2 = 1.0/cali_max2;
00123    cali_fact3 = 1.0/cali_max3;
00124    cali_fact4 = 1.0/cali_max4;
00125    calibrate_go = 0;
00126    statusled=1;
00127    calibrated=true;
00128 }
00129 
00130 /* 
00131 int main()
00132 {
00133     T1.attach(&samplego, (float)1/Fs);
00134     cali_button.rise(&calibrate);
00135     while(1)
00136     {
00137         if(sample_go)
00138         {
00139             sample_filter();
00140             sample_go = 0;
00141         } 
00142     } // while end   
00143 } // main end
00144 */