Mbed bordje 1 -af
Dependencies: Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed
Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by
main.cpp@2:293665548183, 2017-10-24 (annotated)
- Committer:
- RoyvZ
- Date:
- Tue Oct 24 11:58:29 2017 +0000
- Revision:
- 2:293665548183
- Parent:
- 0:b6c8d56842ce
- Child:
- 3:2ffbee47fb38
EMG filtering;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
RoyvZ | 0:b6c8d56842ce | 1 | /** |
RoyvZ | 0:b6c8d56842ce | 2 | * Demo program for BiQuad and BiQuadChain classes |
RoyvZ | 0:b6c8d56842ce | 3 | * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion |
RoyvZ | 0:b6c8d56842ce | 4 | */ |
RoyvZ | 0:b6c8d56842ce | 5 | #include "mbed.h" |
RoyvZ | 0:b6c8d56842ce | 6 | #include "HIDScope.h" |
RoyvZ | 0:b6c8d56842ce | 7 | #include <stdlib.h> |
RoyvZ | 0:b6c8d56842ce | 8 | #include <iostream> |
RoyvZ | 0:b6c8d56842ce | 9 | #include <iomanip> |
RoyvZ | 0:b6c8d56842ce | 10 | #include <complex> |
RoyvZ | 0:b6c8d56842ce | 11 | #include "BiQuad.h" |
RoyvZ | 0:b6c8d56842ce | 12 | |
RoyvZ | 2:293665548183 | 13 | |
RoyvZ | 0:b6c8d56842ce | 14 | AnalogIn emg0( A0 ); |
RoyvZ | 0:b6c8d56842ce | 15 | AnalogIn emg1( A1 ); |
RoyvZ | 2:293665548183 | 16 | DigitalIn button1(PTA4); |
RoyvZ | 0:b6c8d56842ce | 17 | |
RoyvZ | 0:b6c8d56842ce | 18 | Ticker sample_timer; |
RoyvZ | 2:293665548183 | 19 | Ticker cal_timer; |
RoyvZ | 0:b6c8d56842ce | 20 | HIDScope scope( 2 ); |
RoyvZ | 0:b6c8d56842ce | 21 | DigitalOut led(LED1); |
RoyvZ | 2:293665548183 | 22 | //DigitalOut led2(LED_GREEN); |
RoyvZ | 2:293665548183 | 23 | |
RoyvZ | 2:293665548183 | 24 | double S[50]; |
RoyvZ | 2:293665548183 | 25 | double meanSum = 0; |
RoyvZ | 2:293665548183 | 26 | |
RoyvZ | 2:293665548183 | 27 | //Filter toevoegen, bestaande uit notch, high- en lowpass filter |
RoyvZ | 2:293665548183 | 28 | BiQuadChain Notch; |
RoyvZ | 2:293665548183 | 29 | //50 Hz Notch filter, for radiation from surroundings |
RoyvZ | 0:b6c8d56842ce | 30 | |
RoyvZ | 2:293665548183 | 31 | //Notch filter chain for 100 Hz |
RoyvZ | 2:293665548183 | 32 | BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); |
RoyvZ | 2:293665548183 | 33 | BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); |
RoyvZ | 2:293665548183 | 34 | BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); |
RoyvZ | 2:293665548183 | 35 | |
RoyvZ | 2:293665548183 | 36 | //Notch filter chain for 50 Hz |
RoyvZ | 2:293665548183 | 37 | BiQuad bq1( 9.87512e-01, -1.95079e+00, 9.87563e-01, -1.96308e+00, 9.87512e-01 ); |
RoyvZ | 2:293665548183 | 38 | BiQuad bq2( 1.00000e+00, -1.97533e+00, 9.99919e-01, -1.96726e+00, 9.93522e-01 ); |
RoyvZ | 2:293665548183 | 39 | BiQuad bq3( 1.00000e+00, -1.97546e+00, 1.00003e+00, -1.97108e+00, 9.93951e-01 ); |
RoyvZ | 2:293665548183 | 40 | |
RoyvZ | 2:293665548183 | 41 | //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals |
RoyvZ | 2:293665548183 | 42 | //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01); |
RoyvZ | 2:293665548183 | 43 | |
RoyvZ | 2:293665548183 | 44 | //2Hz Highpass |
RoyvZ | 2:293665548183 | 45 | BiQuad Highpass1(9.91154e-01, -1.98231e+00, 9.91154e-01, -1.98223e+00, 9.82385e-01); |
RoyvZ | 0:b6c8d56842ce | 46 | |
RoyvZ | 0:b6c8d56842ce | 47 | |
RoyvZ | 2:293665548183 | 48 | //20 Hz Lowpass |
RoyvZ | 2:293665548183 | 49 | BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 ); |
RoyvZ | 0:b6c8d56842ce | 50 | |
RoyvZ | 2:293665548183 | 51 | //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...) |
RoyvZ | 2:293665548183 | 52 | //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01); |
RoyvZ | 0:b6c8d56842ce | 53 | |
RoyvZ | 0:b6c8d56842ce | 54 | void sample() |
RoyvZ | 0:b6c8d56842ce | 55 | { |
RoyvZ | 0:b6c8d56842ce | 56 | /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */ |
RoyvZ | 2:293665548183 | 57 | //scope.set(0, emg0.read() ); |
RoyvZ | 2:293665548183 | 58 | //scope.set(1, fabs(Notch.step(emg0.read())) ); |
RoyvZ | 2:293665548183 | 59 | scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); |
RoyvZ | 2:293665548183 | 60 | |
RoyvZ | 2:293665548183 | 61 | for (int i=1;i < 50; i++) { |
RoyvZ | 2:293665548183 | 62 | S[i] = S[i-1]; |
RoyvZ | 2:293665548183 | 63 | meanSum = meanSum + S[i]; |
RoyvZ | 2:293665548183 | 64 | } |
RoyvZ | 2:293665548183 | 65 | S[0] = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))); |
RoyvZ | 2:293665548183 | 66 | double mean = meanSum / 50; |
RoyvZ | 2:293665548183 | 67 | scope.set(1, fabs(4*Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))))); |
RoyvZ | 2:293665548183 | 68 | meanSum = 0; |
RoyvZ | 0:b6c8d56842ce | 69 | /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) |
RoyvZ | 0:b6c8d56842ce | 70 | * Ensure that enough channels are available (HIDScope scope( 2 )) |
RoyvZ | 0:b6c8d56842ce | 71 | * Finally, send all channels to the PC at once */ |
RoyvZ | 0:b6c8d56842ce | 72 | scope.send(); |
RoyvZ | 0:b6c8d56842ce | 73 | /* To indicate that the function is working, the LED is toggled */ |
RoyvZ | 0:b6c8d56842ce | 74 | led = !led; |
RoyvZ | 0:b6c8d56842ce | 75 | } |
RoyvZ | 0:b6c8d56842ce | 76 | |
RoyvZ | 2:293665548183 | 77 | void calibration() |
RoyvZ | 2:293665548183 | 78 | { |
RoyvZ | 2:293665548183 | 79 | //function to determine maximal EMG input in order to allow motorcontrol to be activated |
RoyvZ | 2:293665548183 | 80 | double maxsignal = 0; |
RoyvZ | 2:293665548183 | 81 | if (button1.read() == false){ |
RoyvZ | 2:293665548183 | 82 | for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds |
RoyvZ | 2:293665548183 | 83 | double signalfinal = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))); |
RoyvZ | 2:293665548183 | 84 | if (signalfinal >= maxsignal){ |
RoyvZ | 2:293665548183 | 85 | maxsignal = signalfinal; //keep changing the maximal signal |
RoyvZ | 2:293665548183 | 86 | } |
RoyvZ | 2:293665548183 | 87 | } |
RoyvZ | 2:293665548183 | 88 | } |
RoyvZ | 2:293665548183 | 89 | } |
RoyvZ | 2:293665548183 | 90 | |
RoyvZ | 0:b6c8d56842ce | 91 | int main() |
RoyvZ | 0:b6c8d56842ce | 92 | { |
RoyvZ | 2:293665548183 | 93 | //Constructing the notch filter chain |
RoyvZ | 2:293665548183 | 94 | Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ); |
RoyvZ | 2:293665548183 | 95 | |
RoyvZ | 0:b6c8d56842ce | 96 | /**Attach the 'sample' function to the timer 'sample_timer'. |
RoyvZ | 2:293665548183 | 97 | * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz |
RoyvZ | 0:b6c8d56842ce | 98 | */ |
RoyvZ | 2:293665548183 | 99 | sample_timer.attach(&sample, 0.001); |
RoyvZ | 2:293665548183 | 100 | cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated |
RoyvZ | 0:b6c8d56842ce | 101 | |
RoyvZ | 0:b6c8d56842ce | 102 | /*empty loop, sample() is executed periodically*/ |
RoyvZ | 0:b6c8d56842ce | 103 | while(true) { |
RoyvZ | 0:b6c8d56842ce | 104 | |
RoyvZ | 2:293665548183 | 105 | |
RoyvZ | 0:b6c8d56842ce | 106 | } |
RoyvZ | 0:b6c8d56842ce | 107 | |
RoyvZ | 0:b6c8d56842ce | 108 | } |