Biorobotica TIC / Mbed 2 deprecated EMGSignal

EMG filteren

Dependencies:   HIDScope biquadFilter mbed

main.cpp@1:082004527d2e, 2018-10-11 (annotated)

Committer:
Hubertus
Date:
Thu Oct 11 07:56:45 2018 +0000
Revision:
1:082004527d2e
Parent:
0:55a2b57fd35d
Child:
2:5a5a54374f80
Filter is klaar, nu nog de calibratie

Who changed what in which revision?

UserRevisionLine numberNew 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 0:55a2b57fd35d 10 HIDScope scope( 4 );
Hubertus 1:082004527d2e 11 DigitalOut ledred(LED1);
Hubertus 1:082004527d2e 12 DigitalOut ledgreen(LED_GREEN);
Hubertus 1:082004527d2e 13
Hubertus 0:55a2b57fd35d 14
Hubertus 0:55a2b57fd35d 15 //------------Filter parameters----------------------
Hubertus 0:55a2b57fd35d 16
Hubertus 0:55a2b57fd35d 17 //Lowpassfilter
Hubertus 0:55a2b57fd35d 18 const double b0LP = 0.0014831498359569692;
Hubertus 0:55a2b57fd35d 19 const double b1LP = 0.0029662996719139385;
Hubertus 0:55a2b57fd35d 20 const double b2LP = 0.0014831498359569692;
Hubertus 0:55a2b57fd35d 21 const double a1LP = -1.918570032544273;
Hubertus 0:55a2b57fd35d 22 const double a2LP = 0.9245026318881009;
Hubertus 0:55a2b57fd35d 23 //Highpassfilter
Hubertus 0:55a2b57fd35d 24 const double b0HP = 0.9921463375688531;
Hubertus 0:55a2b57fd35d 25 const double b1HP = -1.9842926751377061;
Hubertus 0:55a2b57fd35d 26 const double b2HP = 0.9921463375688531;
Hubertus 0:55a2b57fd35d 27 const double a1HP = -1.9841702689557372;
Hubertus 0:55a2b57fd35d 28 const double a2HP = 0.9844150813196749;
Hubertus 0:55a2b57fd35d 29 //Notchfilter
Hubertus 0:55a2b57fd35d 30 //const double b0NO = 1;
Hubertus 0:55a2b57fd35d 31 //const double b1NO = 1;
Hubertus 0:55a2b57fd35d 32 //const double b2NO = 1;
Hubertus 0:55a2b57fd35d 33 //const double a1NO = 1;
Hubertus 0:55a2b57fd35d 34 //const double a2NO = 1;
Hubertus 0:55a2b57fd35d 35
Hubertus 0:55a2b57fd35d 36 //--------------Filter------------
Hubertus 0:55a2b57fd35d 37 BiQuad LP1( b0LP, b1LP, b2LP, a1LP, a2LP ); //Lowpass filter Biquad
Hubertus 0:55a2b57fd35d 38 BiQuad HP2( b0HP, b1HP, b2HP, a1HP, a2HP ); //Highpass filter Biquad
Hubertus 0:55a2b57fd35d 39 //BiQuad NO3( b0NO, b1NO, b2NO, a1NO, a2NO ); //Notch filter Biquad
Hubertus 0:55a2b57fd35d 40 BiQuadChain BiQuad_filter;
Hubertus 0:55a2b57fd35d 41 float Signal;
Hubertus 0:55a2b57fd35d 42 float Signal_filtered;
Hubertus 1:082004527d2e 43 float Signal_filtered_HP;
Hubertus 1:082004527d2e 44 float Signal_filtered_fabs;
Hubertus 1:082004527d2e 45
Hubertus 1:082004527d2e 46
Hubertus 0:55a2b57fd35d 47
Hubertus 0:55a2b57fd35d 48 /** Sample function
Hubertus 0:55a2b57fd35d 49 * this function samples the emg and sends it to HIDScope
Hubertus 0:55a2b57fd35d 50 **/
Hubertus 1:082004527d2e 51 void sample_filter()
Hubertus 1:082004527d2e 52 {
Hubertus 1:082004527d2e 53 Signal = emg0;
Hubertus 1:082004527d2e 54 Signal_filtered_HP = HP2.step(Signal);
Hubertus 1:082004527d2e 55 Signal_filtered_fabs = fabs(Signal_filtered_HP);
Hubertus 1:082004527d2e 56 Signal_filtered = LP1.step(Signal_filtered_fabs);
Hubertus 1:082004527d2e 57
Hubertus 1:082004527d2e 58
Hubertus 1:082004527d2e 59 //Poging tot goed filter
Hubertus 0:55a2b57fd35d 60 //Signal = emg0-emg1;
Hubertus 0:55a2b57fd35d 61 // BiQuad_filter.add(&LP);
Hubertus 0:55a2b57fd35d 62 // Signal_filtered = BiQuad_filter.step(Signal);
Hubertus 0:55a2b57fd35d 63 // Signal_filtered = fabs(Signal_filtered);
Hubertus 0:55a2b57fd35d 64 // BiQuad_filter.add(&HP);
Hubertus 0:55a2b57fd35d 65 // Signal_filtered = BiQuad_filter.step(Signal_filtered);
Hubertus 0:55a2b57fd35d 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 0:55a2b57fd35d 70 scope.set(1, emg1.read() );
Hubertus 0:55a2b57fd35d 71 scope.set(2, Signal );
Hubertus 0:55a2b57fd35d 72 scope.set(3, 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 0:55a2b57fd35d 81 //----------Parameters voor filter----------
Hubertus 0:55a2b57fd35d 82 //const float Fs = 200.0;
Hubertus 0:55a2b57fd35d 83 //const float Fnyq = Fs / 2;
Hubertus 0:55a2b57fd35d 84 //const int Nthorder = 4;
Hubertus 0:55a2b57fd35d 85 //
Hubertus 0:55a2b57fd35d 86 ////---------Functie voor filter-----------
Hubertus 0:55a2b57fd35d 87 //void filter()
Hubertus 0:55a2b57fd35d 88 //{
Hubertus 0:55a2b57fd35d 89 //
Hubertus 0:55a2b57fd35d 90 //}
Hubertus 0:55a2b57fd35d 91
Hubertus 0:55a2b57fd35d 92 int main()
Hubertus 0:55a2b57fd35d 93 {
Hubertus 0:55a2b57fd35d 94
Hubertus 0:55a2b57fd35d 95 /**Attach the 'sample' function to the timer 'sample_timer'.
Hubertus 0:55a2b57fd35d 96 * this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
Hubertus 0:55a2b57fd35d 97 */
Hubertus 1:082004527d2e 98
Hubertus 1:082004527d2e 99 sample_timer.attach(&sample_filter, 0.002);
Hubertus 0:55a2b57fd35d 100
Hubertus 0:55a2b57fd35d 101 /*empty loop, sample() is executed periodically*/
Hubertus 1:082004527d2e 102 while(1) {
Hubertus 1:082004527d2e 103
Hubertus 1:082004527d2e 104 if (Signal_filtered > 0.1)
Hubertus 1:082004527d2e 105 { ledgreen = 0;
Hubertus 1:082004527d2e 106 }
Hubertus 1:082004527d2e 107 else {
Hubertus 1:082004527d2e 108 ledgreen = 1;
Hubertus 1:082004527d2e 109 }
Hubertus 1:082004527d2e 110
Hubertus 1:082004527d2e 111
Hubertus 1:082004527d2e 112 }
Hubertus 0:55a2b57fd35d 113 }
Hubertus 0:55a2b57fd35d 114
Hubertus 0:55a2b57fd35d 115