Biorobotica TIC / Mbed 2 deprecated EMGSignal

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?

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 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