EMG filteren
Dependencies: HIDScope biquadFilter mbed
main.cpp@2:5a5a54374f80, 2018-10-11 (annotated)
- Committer:
- Hubertus
- Date:
- Thu Oct 11 10:10:22 2018 +0000
- Revision:
- 2:5a5a54374f80
- Parent:
- 1:082004527d2e
Calibratie is in gang
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hubertus | 0:55a2b57fd35d | 1 | #include "mbed.h" |
Hubertus | 0:55a2b57fd35d | 2 | #include "HIDScope.h" |
Hubertus | 0:55a2b57fd35d | 3 | #include "BiQuad.h" |
Hubertus | 0:55a2b57fd35d | 4 | |
Hubertus | 0:55a2b57fd35d | 5 | //Define objects |
Hubertus | 0:55a2b57fd35d | 6 | AnalogIn emg0( A0 ); |
Hubertus | 0:55a2b57fd35d | 7 | AnalogIn emg1( A1 ); |
Hubertus | 0:55a2b57fd35d | 8 | |
Hubertus | 0:55a2b57fd35d | 9 | Ticker sample_timer; |
Hubertus | 2:5a5a54374f80 | 10 | HIDScope scope( 2 ); |
Hubertus | 2:5a5a54374f80 | 11 | DigitalOut ledred(LED_RED); |
Hubertus | 1:082004527d2e | 12 | DigitalOut ledgreen(LED_GREEN); |
Hubertus | 2:5a5a54374f80 | 13 | DigitalOut ledblue(LED_BLUE); |
Hubertus | 2:5a5a54374f80 | 14 | InterruptIn calbutton(PTA4); |
Hubertus | 2:5a5a54374f80 | 15 | |
Hubertus | 2:5a5a54374f80 | 16 | |
Hubertus | 2:5a5a54374f80 | 17 | const double Fs = 500; //Sample frequency |
Hubertus | 2:5a5a54374f80 | 18 | double cal_fact = 1; |
Hubertus | 1:082004527d2e | 19 | |
Hubertus | 0:55a2b57fd35d | 20 | |
Hubertus | 0:55a2b57fd35d | 21 | //------------Filter parameters---------------------- |
Hubertus | 0:55a2b57fd35d | 22 | |
Hubertus | 0:55a2b57fd35d | 23 | //Lowpassfilter |
Hubertus | 0:55a2b57fd35d | 24 | const double b0LP = 0.0014831498359569692; |
Hubertus | 0:55a2b57fd35d | 25 | const double b1LP = 0.0029662996719139385; |
Hubertus | 0:55a2b57fd35d | 26 | const double b2LP = 0.0014831498359569692; |
Hubertus | 0:55a2b57fd35d | 27 | const double a1LP = -1.918570032544273; |
Hubertus | 0:55a2b57fd35d | 28 | const double a2LP = 0.9245026318881009; |
Hubertus | 0:55a2b57fd35d | 29 | //Highpassfilter |
Hubertus | 0:55a2b57fd35d | 30 | const double b0HP = 0.9921463375688531; |
Hubertus | 0:55a2b57fd35d | 31 | const double b1HP = -1.9842926751377061; |
Hubertus | 0:55a2b57fd35d | 32 | const double b2HP = 0.9921463375688531; |
Hubertus | 0:55a2b57fd35d | 33 | const double a1HP = -1.9841702689557372; |
Hubertus | 0:55a2b57fd35d | 34 | const double a2HP = 0.9844150813196749; |
Hubertus | 0:55a2b57fd35d | 35 | //Notchfilter |
Hubertus | 2:5a5a54374f80 | 36 | const double b0NO = 0.7728616577547288; |
Hubertus | 2:5a5a54374f80 | 37 | const double b1NO = -1.2505164308487402; |
Hubertus | 2:5a5a54374f80 | 38 | const double b2NO = 0.7728616577547288; |
Hubertus | 2:5a5a54374f80 | 39 | const double a1NO = -1.2505164308487402; |
Hubertus | 2:5a5a54374f80 | 40 | const double a2NO = 0.5457233155094577; |
Hubertus | 0:55a2b57fd35d | 41 | |
Hubertus | 0:55a2b57fd35d | 42 | //--------------Filter------------ |
Hubertus | 0:55a2b57fd35d | 43 | BiQuad LP1( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad |
Hubertus | 0:55a2b57fd35d | 44 | BiQuad HP2( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad |
Hubertus | 2:5a5a54374f80 | 45 | BiQuad NO3( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad |
Hubertus | 0:55a2b57fd35d | 46 | BiQuadChain BiQuad_filter; |
Hubertus | 2:5a5a54374f80 | 47 | double Signal; |
Hubertus | 2:5a5a54374f80 | 48 | double Signal_filtered; |
Hubertus | 2:5a5a54374f80 | 49 | double Signal_filtered_HP; |
Hubertus | 2:5a5a54374f80 | 50 | double Signal_filtered_fabs; |
Hubertus | 2:5a5a54374f80 | 51 | double Signal_filtered_LP; |
Hubertus | 1:082004527d2e | 52 | |
Hubertus | 1:082004527d2e | 53 | |
Hubertus | 0:55a2b57fd35d | 54 | |
Hubertus | 2:5a5a54374f80 | 55 | /** Sample filter function |
Hubertus | 2:5a5a54374f80 | 56 | * this function samples the emg, filters it and sends it to HIDScope |
Hubertus | 0:55a2b57fd35d | 57 | **/ |
Hubertus | 1:082004527d2e | 58 | void sample_filter() |
Hubertus | 1:082004527d2e | 59 | { |
Hubertus | 1:082004527d2e | 60 | Signal = emg0; |
Hubertus | 1:082004527d2e | 61 | Signal_filtered_HP = HP2.step(Signal); |
Hubertus | 1:082004527d2e | 62 | Signal_filtered_fabs = fabs(Signal_filtered_HP); |
Hubertus | 2:5a5a54374f80 | 63 | Signal_filtered_LP = LP1.step(Signal_filtered_fabs); |
Hubertus | 2:5a5a54374f80 | 64 | Signal_filtered = NO3.step(Signal_filtered_LP) * cal_fact; |
Hubertus | 1:082004527d2e | 65 | |
Hubertus | 2:5a5a54374f80 | 66 | |
Hubertus | 0:55a2b57fd35d | 67 | //Signal_filtered = fabs(Signal_filtered); // je wil alleen de absolute waardes |
Hubertus | 0:55a2b57fd35d | 68 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
Hubertus | 0:55a2b57fd35d | 69 | scope.set(0, emg0.read() ); |
Hubertus | 2:5a5a54374f80 | 70 | //scope.set(1, emg1.read() ); |
Hubertus | 2:5a5a54374f80 | 71 | //scope.set(2, Signal ); |
Hubertus | 2:5a5a54374f80 | 72 | scope.set(1, Signal_filtered); |
Hubertus | 0:55a2b57fd35d | 73 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
Hubertus | 0:55a2b57fd35d | 74 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
Hubertus | 0:55a2b57fd35d | 75 | * Finally, send all channels to the PC at once */ |
Hubertus | 0:55a2b57fd35d | 76 | scope.send(); |
Hubertus | 0:55a2b57fd35d | 77 | /* To indicate that the function is working, the LED is toggled */ |
Hubertus | 1:082004527d2e | 78 | ledred = !ledred; |
Hubertus | 0:55a2b57fd35d | 79 | } |
Hubertus | 0:55a2b57fd35d | 80 | |
Hubertus | 2:5a5a54374f80 | 81 | void calibratie() |
Hubertus | 2:5a5a54374f80 | 82 | { |
Hubertus | 2:5a5a54374f80 | 83 | ledblue = 0; |
Hubertus | 2:5a5a54374f80 | 84 | cal_fact = 1; |
Hubertus | 2:5a5a54374f80 | 85 | double cal_max = 0; |
Hubertus | 2:5a5a54374f80 | 86 | |
Hubertus | 2:5a5a54374f80 | 87 | for(int i = 0; i < 5*Fs; i++) |
Hubertus | 2:5a5a54374f80 | 88 | { |
Hubertus | 2:5a5a54374f80 | 89 | sample_filter(); |
Hubertus | 2:5a5a54374f80 | 90 | if(Signal_filtered > cal_max) |
Hubertus | 2:5a5a54374f80 | 91 | { |
Hubertus | 2:5a5a54374f80 | 92 | cal_max = Signal_filtered; |
Hubertus | 2:5a5a54374f80 | 93 | } |
Hubertus | 2:5a5a54374f80 | 94 | wait(1/(float)Fs); |
Hubertus | 2:5a5a54374f80 | 95 | } |
Hubertus | 2:5a5a54374f80 | 96 | cal_fact = 1.0/cal_max; |
Hubertus | 2:5a5a54374f80 | 97 | ledblue = 1; |
Hubertus | 2:5a5a54374f80 | 98 | } |
Hubertus | 0:55a2b57fd35d | 99 | |
Hubertus | 0:55a2b57fd35d | 100 | int main() |
Hubertus | 0:55a2b57fd35d | 101 | { |
Hubertus | 0:55a2b57fd35d | 102 | |
Hubertus | 0:55a2b57fd35d | 103 | /**Attach the 'sample' function to the timer 'sample_timer'. |
Hubertus | 0:55a2b57fd35d | 104 | * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz |
Hubertus | 0:55a2b57fd35d | 105 | */ |
Hubertus | 2:5a5a54374f80 | 106 | ledblue = 1; |
Hubertus | 2:5a5a54374f80 | 107 | sample_timer.attach(&sample_filter, 1/(float)Fs); |
Hubertus | 2:5a5a54374f80 | 108 | calbutton.rise(&calibratie); |
Hubertus | 0:55a2b57fd35d | 109 | |
Hubertus | 0:55a2b57fd35d | 110 | /*empty loop, sample() is executed periodically*/ |
Hubertus | 1:082004527d2e | 111 | while(1) { |
Hubertus | 1:082004527d2e | 112 | |
Hubertus | 2:5a5a54374f80 | 113 | if (Signal_filtered > 0.7) |
Hubertus | 1:082004527d2e | 114 | { ledgreen = 0; |
Hubertus | 1:082004527d2e | 115 | } |
Hubertus | 1:082004527d2e | 116 | else { |
Hubertus | 1:082004527d2e | 117 | ledgreen = 1; |
Hubertus | 1:082004527d2e | 118 | } |
Hubertus | 1:082004527d2e | 119 | |
Hubertus | 1:082004527d2e | 120 | |
Hubertus | 1:082004527d2e | 121 | } |
Hubertus | 0:55a2b57fd35d | 122 | } |
Hubertus | 0:55a2b57fd35d | 123 | |
Hubertus | 0:55a2b57fd35d | 124 |