main.cpp

00001 #include "mbed.h"
00002 #include "BiQuad.h"
00003 #include "HIDScope.h"
00004 //Enum with states
00005 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};
00006 
00007 //Variable called 'state'
00008 states state = STATE_DEFAULT;
00009 
00010 //Creating two scope channels
00011 HIDScope scope(6);
00012 
00013 //Notch filter
00014 BiQuadChain notch_50_0;
00015 BiQuadChain notch_50_1;
00016 BiQuadChain notch_50_2;
00017 BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
00018 BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
00019 BiQuad bq5( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
00020 
00021 //High pass filter
00022 BiQuadChain high_pass_0;
00023 BiQuadChain high_pass_1;
00024 BiQuadChain high_pass_2;
00025 BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
00026 BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
00027 //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
00028 
00029 //Low pass filter
00030 BiQuadChain low_pass_0;
00031 BiQuadChain low_pass_1;
00032 BiQuadChain low_pass_2;
00033 BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
00034 BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
00035 BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
00036 
00037 
00038 //Ticker
00039 Ticker emgSampleTicker;
00040 
00041 //Timeout to change state after 5 seconds
00042 Timeout change_state;
00043 
00044 //Timeout to change state after 15 seconds
00045 Timeout change_state2;
00046 
00047 //LED
00048 DigitalOut led(LED_RED);
00049 
00050 //Emg input
00051 AnalogIn emg0( A0 );
00052 AnalogIn emg1( A1 );
00053 AnalogIn emg2( A2 );
00054 
00055 //Go flag for emg sample
00056 bool go_emgSample;
00057 
00058 //Variables for intermediate filter values
00059 double emg_sample[3];
00060 double emg_low_passed_200[3];
00061 double emg_notch[3];
00062 double emg_high_passed[3];
00063 double emg_low_passed[3];
00064 double min_emg[3];
00065 double max_emg[3];
00066 
00067 //Calculating normalized outputs
00068 double input_force0;
00069 double input_force1;
00070 double input_force2;
00071 
00072 //count for emg min max
00073 int start_calibration = 0;
00074 
00075 void emgSample() {
00076     go_emgSample = true;
00077 }
00078 
00079 void calibrate() {
00080     state = STATE_CALIBRATION;  
00081     led.write(0);
00082 }
00083 
00084 void run() {
00085     state = STATE_RUN;  
00086     led.write(1);
00087 }
00088 
00089 void EMG_filter();
00090 
00091 int main() {
00092     //combine biquads in biquad chains for notch/high- low-pass filters
00093     notch_50_0.add( &bq3 ).add( &bq4 ).add( &bq5 );
00094     notch_50_1.add( &bq3 ).add( &bq4 ).add( &bq5 );
00095     notch_50_2.add( &bq3 ).add( &bq4 ).add( &bq5 );
00096     high_pass_0.add( &bq6 ).add( &bq7 );
00097     high_pass_1.add( &bq6 ).add( &bq7 );
00098     high_pass_2.add( &bq6 ).add( &bq7 );
00099     low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
00100     low_pass_1.add( &bq9 ).add( &bq10 ).add( &bq11 );
00101     low_pass_2.add( &bq9 ).add( &bq10 ).add( &bq11 );
00102     led.write(1);
00103     
00104     change_state.attach( &calibrate,5);
00105     change_state2.attach( &run,15);
00106     emgSampleTicker.attach( &emgSample, 0.005); //200Hz
00107     while( true ){
00108         if(go_emgSample == true){
00109             EMG_filter();
00110         }
00111    }     
00112 }
00113 
00114 
00115 void EMG_filter() {
00116     if(go_emgSample == true){
00117             //read the emg signal
00118             emg_sample[0] = emg0.read();
00119             emg_sample[1] = emg1.read();
00120             emg_sample[2] = emg2.read();
00121             //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]); 
00122             //filter out the 50Hz components with a notch filter
00123             emg_notch[0] = notch_50_0.step(emg_sample[0]);
00124             emg_notch[1] = notch_50_1.step(emg_sample[1]);
00125             emg_notch[2] = notch_50_2.step(emg_sample[2]);
00126             //high pass the signal (removing motion artifacts and offset)
00127             emg_high_passed[0] = high_pass_0.step(emg_notch[0]);
00128             emg_high_passed[1] = high_pass_1.step(emg_notch[1]);
00129             emg_high_passed[2] = high_pass_2.step(emg_notch[2]);    
00130             float emg_fabs[3];
00131             emg_fabs[0] = fabs(emg_high_passed[0]);
00132             emg_fabs[1] = fabs(emg_high_passed[1]);
00133             emg_fabs[2] = fabs(emg_high_passed[2]);
00134             //low pass the rectified emg signal
00135             emg_low_passed[0] = low_pass_0.step(emg_fabs[0]);
00136             emg_low_passed[1] = low_pass_1.step(emg_fabs[1]);
00137             emg_low_passed[2] = low_pass_2.step(emg_fabs[2]);
00138            // pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]);
00139             //Calculating min value and max value of emg signal
00140             if(state == STATE_CALIBRATION)
00141             {
00142                 if (start_calibration == 0) {
00143                     min_emg[0] = emg_low_passed[0];
00144                     max_emg[0] = emg_low_passed[0];
00145                     min_emg[1] = emg_low_passed[1];
00146                     max_emg[1] = emg_low_passed[1];
00147                     min_emg[2] = emg_low_passed[2];
00148                     max_emg[2] = emg_low_passed[2];
00149                     start_calibration++;
00150                 }
00151                 else {
00152                     //finding min and max of emg0
00153                     if (emg_low_passed[0] < min_emg[0]) {
00154                         min_emg[0] = emg_low_passed[0];
00155                     }
00156                     else if (emg_low_passed[0] > max_emg[0]) {
00157                         max_emg[0] = emg_low_passed[0];
00158                     }
00159                     
00160                     //finding min and max of emg1
00161                     if (emg_low_passed[1] < min_emg[1]) {
00162                         min_emg[1] = emg_low_passed[1];
00163                     }
00164                     else if (emg_low_passed[1] > max_emg[1]) {
00165                         max_emg[1] = emg_low_passed[1];
00166                     }
00167                     
00168                     //finding min and max of emg2
00169                     if (emg_low_passed[2] < min_emg[2]) {
00170                         min_emg[2] = emg_low_passed[2];
00171                     }
00172                     else if (emg_low_passed[2] > max_emg[2]) {
00173                         max_emg[2] = emg_low_passed[2];
00174                     }
00175                 }   
00176             } 
00177             
00178             //calculating input_forces for controller
00179             input_force0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
00180             input_force1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
00181             input_force2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
00182             
00183             //Send scope data
00184             scope.set(0,emg_high_passed[0]);
00185             scope.set(1,emg_high_passed[1]);
00186             scope.set(2,emg_high_passed[2]);
00187             scope.set(3,emg_low_passed[0]);
00188             scope.set(4,emg_low_passed[1]);
00189             scope.set(5,emg_low_passed[2]);
00190             //scope.set(3,input_force0);
00191             //scope.set(2,emg_low_passed[0]);
00192             //scope.set(3,input_force2);
00193             scope.send();
00194 
00195             go_emgSample = false;
00196         }   
00197     }
00198