Gaston Gabriël / Mbed 2 deprecated BiQuadImplementationGroep6

probeer maar wat

Dependencies:   HIDScope biquadFilter mbed

main.cpp@1:1fd8a80ad5df, 2018-10-30 (annotated)

Committer:
gastongab
Date:
Tue Oct 30 09:15:19 2018 +0000
Revision:
1:1fd8a80ad5df
Parent:
0:83108f9ff38b 
dit is een commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gastongab 0:83108f9ff38b 1 #include "mbed.h"
gastongab 0:83108f9ff38b 2 #include "HIDScope.h"
gastongab 0:83108f9ff38b 3 #include "biquadFilter.h"
gastongab 0:83108f9ff38b 4
gastongab 0:83108f9ff38b 5 //Input system
gastongab 0:83108f9ff38b 6 AnalogIn emg1(A0); //right biceps
gastongab 0:83108f9ff38b 7 AnalogIn emg2(A1); //right triceps
gastongab 0:83108f9ff38b 8 AnalogIn emg3(A2); //left biceps
gastongab 0:83108f9ff38b 9 AnalogIn emg4(A3); //left triceps
gastongab 0:83108f9ff38b 10
gastongab 0:83108f9ff38b 11 DigitalOut ledje(LED_GREEN);
gastongab 0:83108f9ff38b 12
gastongab 0:83108f9ff38b 13 //EMG tickers, these tickers are called in the mainscript with fsample 500Hz, also sends to HIDscope with same fsample
gastongab 0:83108f9ff38b 14 Ticker sample_ticker;
gastongab 0:83108f9ff38b 15 Ticker main_ticker;
gastongab 0:83108f9ff38b 16
gastongab 0:83108f9ff38b 17
gastongab 0:83108f9ff38b 18 //HIDscope to check whether filter is functional or not
gastongab 0:83108f9ff38b 19 HIDScope scope(2); // aantal kanalen voor je HIDScope
gastongab 0:83108f9ff38b 20
gastongab 0:83108f9ff38b 21 //4th order highpass filter with fc = 20 Hz so that means there are 2 biquad chains
gastongab 0:83108f9ff38b 22
gastongab 0:83108f9ff38b 23 const double highp_1_a1 = -1.99997409063390;
gastongab 0:83108f9ff38b 24 const double highp_1_a2= 0.999975877656141;
gastongab 0:83108f9ff38b 25 const double highp_1_b0 = 0.0920510808414248;
gastongab 0:83108f9ff38b 26 const double highp_1_b1= -0.184501278881386;
gastongab 0:83108f9ff38b 27 const double highp_1_b2 = 0.0924506332000235;
gastongab 0:83108f9ff38b 28
gastongab 0:83108f9ff38b 29 const double highp_2_a1 = -1.99998249672404;
gastongab 0:83108f9ff38b 30 const double highp_2_a2 = 0.999982653918482;
gastongab 0:83108f9ff38b 31 const double highp_2_b0 = 1;
gastongab 0:83108f9ff38b 32 const double highp_2_b1 = -2.00367218495316;
gastongab 0:83108f9ff38b 33 const double highp_2_b2 = 1.00367558370516;
gastongab 0:83108f9ff38b 34
gastongab 0:83108f9ff38b 35 //notchfilter 50Hz
gastongab 0:83108f9ff38b 36 const double notch_1_a1 = 1.00000018882502;
gastongab 0:83108f9ff38b 37 const double notch_1_a2= -1.97024348390877;
gastongab 0:83108f9ff38b 38 const double notch_1_b0= 0.995504923090779;
gastongab 0:83108f9ff38b 39 const double notch_1_b1= 1;
gastongab 0:83108f9ff38b 40 const double notch_1_b2= -1.97538661427765;
gastongab 0:83108f9ff38b 41
gastongab 0:83108f9ff38b 42 const double notch_2_a1 = 1.00000018882502;
gastongab 0:83108f9ff38b 43 const double notch_2_a2 = -1.97175302143845;
gastongab 0:83108f9ff38b 44 const double notch_2_b0 = 0.995629024908953;
gastongab 0:83108f9ff38b 45 const double notch_2_b1 = 1;
gastongab 0:83108f9ff38b 46 const double notch_2_b2 = -1.97538624436946;
gastongab 0:83108f9ff38b 47
gastongab 0:83108f9ff38b 48 //Enveloping, 4th order low pass filter is used with 3Hz cutoff
gastongab 0:83108f9ff38b 49 const double lowp_1_a1 = -2.00357645725849;
gastongab 0:83108f9ff38b 50 const double lowp_1_a2= 1.00357981243408;
gastongab 0:83108f9ff38b 51 const double lowp_1_b0 = 0.000148081325059440;
gastongab 0:83108f9ff38b 52 const double lowp_1_b1 = -0.000295530099020586;
gastongab 0:83108f9ff38b 53 const double lowp_1_b2 = 0.000147449452281968;
gastongab 0:83108f9ff38b 54
gastongab 0:83108f9ff38b 55 const double lowp_2_a1 = -2.00319253084618;
gastongab 0:83108f9ff38b 56 const double lowp_2_a2 = 1.00319510326279;
gastongab 0:83108f9ff38b 57 const double lowp_2_b0 = 1;
gastongab 0:83108f9ff38b 58 const double lowp_2_b1 = -1.99627963867269;
gastongab 0:83108f9ff38b 59 const double lowp_2_b2 = 0.996283117943831;
gastongab 0:83108f9ff38b 60
gastongab 0:83108f9ff38b 61 //For every EMG signal a set of biquad cascades
gastongab 0:83108f9ff38b 62 //EMG1
gastongab 0:83108f9ff38b 63 biquadFilter highpass1_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);
gastongab 0:83108f9ff38b 64 biquadFilter highpass1_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2); // removes artifacts
gastongab 0:83108f9ff38b 65 biquadFilter notch1_1( notch_1_a1 , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 ); // removes 49-51 Hz power interference
gastongab 0:83108f9ff38b 66 biquadFilter notch1_2( notch_2_a1 , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 ); //
gastongab 0:83108f9ff38b 67 biquadFilter lowpass1_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 ); // EMG envelope
gastongab 0:83108f9ff38b 68 biquadFilter lowpass1_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 );
gastongab 0:83108f9ff38b 69 //EMG2
gastongab 0:83108f9ff38b 70 biquadFilter highpass2_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);
gastongab 0:83108f9ff38b 71 biquadFilter highpass2_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);
gastongab 0:83108f9ff38b 72 biquadFilter notch2_1( notch_1_a1 , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 ); // removes 49-51 Hz power interference
gastongab 0:83108f9ff38b 73 biquadFilter notch2_2( notch_2_a1 , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 ); //
gastongab 0:83108f9ff38b 74 biquadFilter lowpass2_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 ); // EMG envelope
gastongab 0:83108f9ff38b 75 biquadFilter lowpass2_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 );
gastongab 0:83108f9ff38b 76 //EMG3
gastongab 0:83108f9ff38b 77 biquadFilter highpass3_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);
gastongab 0:83108f9ff38b 78 biquadFilter highpass3_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);
gastongab 0:83108f9ff38b 79 biquadFilter notch3_1( notch_1_a1 , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 ); // removes 49-51 Hz power interference
gastongab 0:83108f9ff38b 80 biquadFilter notch3_2( notch_2_a1 , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 ); //
gastongab 0:83108f9ff38b 81 biquadFilter lowpass3_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 ); // EMG envelope
gastongab 0:83108f9ff38b 82 biquadFilter lowpass3_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 );
gastongab 0:83108f9ff38b 83 //EMG4
gastongab 0:83108f9ff38b 84 biquadFilter highpass4_1( highp_1_a1 , highp_1_a2 , highp_1_b0 , highp_1_b1 , highp_1_b2);
gastongab 0:83108f9ff38b 85 biquadFilter highpass4_2( highp_2_a1 , highp_2_a2 , highp_2_b0 , highp_2_b1 , highp_2_b2);
gastongab 0:83108f9ff38b 86 biquadFilter notch4_1( notch_1_a1 , notch_1_a2 , notch_1_b0 , notch_1_b1 , notch_1_b2 ); // removes 49-51 Hz power interference
gastongab 0:83108f9ff38b 87 biquadFilter notch4_2( notch_2_a1 , notch_2_a2 , notch_2_b0 , notch_2_b1 , notch_2_b2 ); //
gastongab 0:83108f9ff38b 88 biquadFilter lowpass4_1( lowp_1_a1 , lowp_1_a2 , lowp_1_b0 , lowp_1_b1 , lowp_1_b2 ); // EMG envelope
gastongab 0:83108f9ff38b 89 biquadFilter lowpass4_2( lowp_2_a1 , lowp_2_a2 , lowp_2_b0 , lowp_2_b1 , lowp_2_b2 );
gastongab 0:83108f9ff38b 90
gastongab 0:83108f9ff38b 91 //Ticker executes this bit
gastongab 0:83108f9ff38b 92 double emg1_filtered, emg2_filtered, emg3_filtered, emg4_filtered;
gastongab 0:83108f9ff38b 93
gastongab 0:83108f9ff38b 94 void emg_samples()
gastongab 0:83108f9ff38b 95 {
gastongab 0:83108f9ff38b 96 //EMG1
gastongab 0:83108f9ff38b 97 double y1_1 = highpass1_1.step ( emg1.read());
gastongab 0:83108f9ff38b 98 double y1_2 = highpass1_2.step ( y1_1);
gastongab 0:83108f9ff38b 99 double y1_3 = notch1_1.step (y1_2);
gastongab 0:83108f9ff38b 100 double y1_4 = notch1_2.step (y1_3);
gastongab 0:83108f9ff38b 101 double y1_5 = abs(y1_4);
gastongab 0:83108f9ff38b 102 double y1_6 = lowpass1_1.step (y1_5);
gastongab 0:83108f9ff38b 103 emg1_filtered = lowpass1_2.step (y1_6);
gastongab 0:83108f9ff38b 104
gastongab 0:83108f9ff38b 105 //EMG2
gastongab 0:83108f9ff38b 106 //double y2_1 = highpass2_1.step ( emg2.read());
gastongab 0:83108f9ff38b 107 //double y2_2 = highpass2_2.step ( y2_1);
gastongab 0:83108f9ff38b 108 //double y2_3 = notch2_1.step (y2_2);
gastongab 0:83108f9ff38b 109 //double y2_4 = notch2_2.step (y2_3);
gastongab 0:83108f9ff38b 110 //double y2_5 = abs(y2_4);
gastongab 0:83108f9ff38b 111 //double y2_6 = lowpass2_1.step (y2_5);
gastongab 0:83108f9ff38b 112 //emg2_filtered = lowpass2_2.step (y2_6);
gastongab 0:83108f9ff38b 113 //EMG3
gastongab 0:83108f9ff38b 114 //double y3_1 = highpass3_1.step ( emg3.read());
gastongab 0:83108f9ff38b 115 //double y3_2 = highpass3_2.step ( y3_1);
gastongab 0:83108f9ff38b 116 //double y3_3 = notch3_1.step (y3_2);
gastongab 0:83108f9ff38b 117 //double y3_4 = notch3_2.step (y3_3);
gastongab 0:83108f9ff38b 118 //double y3_5 = abs(y3_4);
gastongab 0:83108f9ff38b 119 //double y3_6 = lowpass3_1.step (y3_5);
gastongab 0:83108f9ff38b 120 //emg3_filtered = lowpass3_2.step (y3_6);
gastongab 0:83108f9ff38b 121 //EMG4
gastongab 0:83108f9ff38b 122 //double y4_1 = highpass4_1.step ( emg4.read());
gastongab 0:83108f9ff38b 123 //double y4_2 = highpass4_2.step ( y4_1);
gastongab 0:83108f9ff38b 124 //double y4_3 = notch4_1.step (y4_2);
gastongab 0:83108f9ff38b 125 //double y4_4 = notch4_2.step (y4_3);
gastongab 0:83108f9ff38b 126 //double y4_5 = abs(y4_4);
gastongab 0:83108f9ff38b 127 //double y4_6 = lowpass4_1.step (y4_5);
gastongab 0:83108f9ff38b 128 //emg4_filtered = lowpass4_1.step (y4_6);
gastongab 0:83108f9ff38b 129
gastongab 0:83108f9ff38b 130 //Send scope value to HIDScope
gastongab 0:83108f9ff38b 131
gastongab 0:83108f9ff38b 132 //scope.set(2, emg2_filtered);
gastongab 0:83108f9ff38b 133 //scope.set(3, emg3_filtered);
gastongab 0:83108f9ff38b 134
gastongab 0:83108f9ff38b 135
gastongab 0:83108f9ff38b 136 ledje = !ledje;
gastongab 0:83108f9ff38b 137 }
gastongab 0:83108f9ff38b 138
gastongab 0:83108f9ff38b 139 void setscope(){
gastongab 1:1fd8a80ad5df 140 scope.set(0,emg1.read());
gastongab 0:83108f9ff38b 141 scope.set(1,emg1_filtered);
gastongab 0:83108f9ff38b 142 scope.send();
gastongab 0:83108f9ff38b 143 }
gastongab 0:83108f9ff38b 144
gastongab 0:83108f9ff38b 145 int main()
gastongab 0:83108f9ff38b 146 {
gastongab 0:83108f9ff38b 147 sample_ticker.attach(&emg_samples,0.002); //fsample = 500Hz for every ticker
gastongab 1:1fd8a80ad5df 148 main_ticker.attach(&setscope,0.1);
gastongab 0:83108f9ff38b 149 while (true){}
gastongab 0:83108f9ff38b 150 }