main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "biquadFilter.h" 
00004 
00005 AnalogIn emg1(A0); //onderste board
00006 AnalogIn emg2(A1); // 2e board
00007 AnalogIn emg3(A2); // 3e board
00008 AnalogIn emg4(A3); // bovenste board
00009 
00010 Ticker sample_ticker; // naam van de emg-ticker
00011 
00012 HIDScope scope(3);  // aantal kanalen voor je HIDScope
00013 
00014 //highpass filter 20 Hz
00015 const double numhigh_1 = 0.956543225556877;
00016 const double numhigh_2 = -1.91308645113754;
00017 const double numhigh_3 = 0.956543225556877;
00018 const double denhigh_2 = -1.91197067426073;
00019 const double denhigh_3 = 0.9149758348014341;
00020 
00021 //notchfilter 50Hz
00022 //biquad 1
00023 const double numnotch_1_1 = 1;
00024 const double numnotch_1_2 = -1.97538661427765;
00025 const double numnotch_1_3 = 1.00000018882502;
00026 const double dennotch_1_2 = -1.97024348390877;
00027 const double dennotch_1_3 = 0.995504923090779;
00028 
00029 //biquad 2
00030 const double numnotch_2_1 = 1;
00031 const double numnotch_2_2 = -1.97538624436946;
00032 const double numnotch_2_3 = 1.00000018882502;
00033 const double dennotch_2_2 = -1.97175302143845;
00034 const double dennotch_2_3 = 0.995629024908953;
00035 
00036 //lowpass filter 7 Hz  - envelop
00037 const double numlow_1 = 0.000119046743110057;
00038 const double numlow_2 = 0.000238093486220118;
00039 const double numlow_3 = 0.000119046743110057;
00040 const double denlow_2 = -1.968902268531908;
00041 const double denlow_3 = 0.9693784555043481;
00042 
00043 
00044 double u1 = emg1.read();                //inputvariables
00045 double u2 = emg2.read();
00046 double u3 = emg3.read();
00047 double u4 = emg4.read();
00048 
00049 
00050 biquadFilter     highpass( denhigh_2 , denhigh_3 , numhigh_1 , numhigh_2 , numhigh_3);                // removes artifacts
00051 biquadFilter     notch1( dennotch_1_2 , dennotch_1_3 , numnotch_1_1 , numnotch_1_2 , numnotch_1_3 );  // removes 49-51 Hz power interference
00052 biquadFilter     notch2( dennotch_2_2 , dennotch_2_3 , numnotch_2_1 , numnotch_2_2 , numnotch_2_3 );   //
00053 biquadFilter     lowpass( denlow_2 , denlow_3 , numlow_1 , numlow_2 , numlow_3 );                     // EMG envelope 
00054 
00055 biquadFilter     highpass_2( denhigh_2 , denhigh_3 , numhigh_1 , numhigh_2 , numhigh_3);
00056 biquadFilter     notch1_2( dennotch_1_2 , dennotch_1_3 , numnotch_1_1 , numnotch_1_2 , numnotch_1_3 );  // removes 49-51 Hz power interference
00057 biquadFilter     notch2_2( dennotch_2_2 , dennotch_2_3 , numnotch_2_1 , numnotch_2_2 , numnotch_2_3 );   //
00058 biquadFilter     lowpass_2( denlow_2 , denlow_3 , numlow_1 , numlow_2 , numlow_3 );                     // EMG envelope 
00059 
00060 biquadFilter     highpass_3( denhigh_2 , denhigh_3 , numhigh_1 , numhigh_2 , numhigh_3);
00061 biquadFilter     notch1_3( dennotch_1_2 , dennotch_1_3 , numnotch_1_1 , numnotch_1_2 , numnotch_1_3 );  // removes 49-51 Hz power interference
00062 biquadFilter     notch2_3( dennotch_2_2 , dennotch_2_3 , numnotch_2_1 , numnotch_2_2 , numnotch_2_3 );   //
00063 biquadFilter     lowpass_3( denlow_2 , denlow_3 , numlow_1 , numlow_2 , numlow_3 );                     // EMG envelope 
00064 
00065 biquadFilter     highpass_4( denhigh_2 , denhigh_3 , numhigh_1 , numhigh_2 , numhigh_3);
00066 biquadFilter     notch1_4( dennotch_1_2 , dennotch_1_3 , numnotch_1_1 , numnotch_1_2 , numnotch_1_3 );  // removes 49-51 Hz power interference
00067 biquadFilter     notch2_4( dennotch_2_2 , dennotch_2_3 , numnotch_2_1 , numnotch_2_2 , numnotch_2_3 );   //
00068 biquadFilter     lowpass_4( denlow_2 , denlow_3 , numlow_1 , numlow_2 , numlow_3 );                     // EMG envelope 
00069 
00070 void myController()
00071 {
00072 
00073 double y1_1 = highpass.step (u1);
00074 double y2_1 = notch1.step (y1_1);
00075 double y3_1 = notch2.step (y2_1);
00076 double y4_1 = abs(y3_1);
00077 double y5_1 = lowpass.step (y4_1);
00078     
00079 double y1_2 = highpass_2.step (u2);
00080 double y2_2 = notch1_2.step (y1_2);
00081 double y3_2 = notch2_2.step (y2_2);
00082 double y4_2 = abs(y3_2);
00083 double y5_2 = lowpass_2.step (y4_2);
00084     
00085 double y1_3 =  highpass_3.step (u3);
00086 double y2_3 =  notch1_3.step (y1_3);
00087 double y3_3 =  notch2_3.step (y2_3);
00088 double y4_3 =  abs(y3_3);
00089 double y5_3 =  lowpass_3.step (y4_3);
00090 
00091     
00092 double y1_4 =  highpass_4.step (u4);
00093 double y2_4 =  notch1_4.step (y1_4);
00094 double y3_4 =  notch2_4.step (y2_4);
00095 double y4_4 =  abs(y3_4);
00096 double y5_4 =  lowpass_4.step (y4_4);
00097 
00098 
00099 scope.set(0,y5_1);
00100 scope.set(1,y5_2);
00101 scope.set(2,y5_3);
00102 //scope.set(3,y5_4);
00103 scope.send();
00104 }
00105 
00106 int main()
00107 {
00108    sample_ticker.attach(&myController,0.002);
00109    while (true){   
00110     }
00111 }